#include <CParticleFilter.h>
Public Member Functions | |
void | allocate (int num_particles) |
mcl::pose | averageOverNBestAndRandomize (int N, int M=0, float vx=0, float vy=0, float va=0) |
CParticleFilter () | |
default constructor | |
mcl::pose | getDistributionMean (bool doWeighting=false) |
Eigen::Matrix3d | getDistributionVariances () |
void | initializeNormalRandom (mcl::pose p0, mcl::pose variance, int size) |
void | initializeUniform (mcl::pose Pmin, mcl::pose Pmax, mcl::pose dP) |
void | myfree () |
void | normalize () |
void | predict (mcl::pose dP, mcl::pose std) |
void | predict (mcl::pose dP, float Q[4]) |
void | print () |
void | resize (int n) |
void | saveToFile (int Fileind) |
void | SIRUpdate () |
void | updateLikelihood (float *lik) |
~CParticleFilter () | |
destructor | |
Public Attributes | |
mcl::pose | average |
Distribution mean. | |
bool | isAvgSet |
Tells if the distribution mean has been calculated. | |
float | Lik |
Likelihood of the distribution. | |
ownRandom | myrand |
The random number class. | |
int | NumOfParticles |
Number of particles in distribution. | |
float | outLiers |
Percentage of the outlier measurements in distribution. | |
TPoseParticle * | Particles |
Particle distribution. | |
int | size |
reserved memory Size (number of reserved particles!) | |
mcl::pose | variance |
Distribution variance. | |
Private Member Functions | |
void | hpsrt (int *indx) |
Private Attributes | |
TPoseParticle * | tmp |
Particle distribution for SIR (Maintained for efficiency reasons here) |
The Particle filter
Definition at line 358 of file CParticleFilter.h.
default constructor
Definition at line 9 of file CParticleFilter.cpp.
destructor
Definition at line 17 of file CParticleFilter.cpp.
void CParticleFilter::allocate | ( | int | num_particles | ) |
Allocates memory for the distribution num_particles
The amount of particles you want to reserve
Allocates memory for the distribution. You should do this before using the filter num_particles
The amount of particles you want to reserve
Definition at line 25 of file CParticleFilter.cpp.
mcl::pose CParticleFilter::averageOverNBestAndRandomize | ( | int | N, |
int | M = 0 , |
||
float | vx = 0 , |
||
float | vy = 0 , |
||
float | va = 0 |
||
) |
Computes the best indices and returns the average
N | The best particles |
M | Option to randomize the worst M |
vx | variance x |
vy | variance y |
va | variance a |
< find indices for the best ones
Definition at line 519 of file CParticleFilter.cpp.
mcl::pose CParticleFilter::getDistributionMean | ( | bool | doWeighting = false | ) |
Get the mean values of the distribution
Definition at line 59 of file CParticleFilter.cpp.
Eigen::Matrix3d CParticleFilter::getDistributionVariances | ( | ) |
Calculate the variance of the distribution
Calculate the variance of the distribution //xx xy xa// //xy yy ya// //xa ya aa//
Definition at line 113 of file CParticleFilter.cpp.
void CParticleFilter::hpsrt | ( | int * | indx | ) | [private] |
Heap Sort algorithm
*indx | The index of values after sort |
N | The number of values |
Definition at line 565 of file CParticleFilter.cpp.
void CParticleFilter::initializeNormalRandom | ( | mcl::pose | p0, |
mcl::pose | variance, | ||
int | size | ||
) |
Initializes the filter by sampling from normal distribution with mean in and variance defined by
Definition at line 157 of file CParticleFilter.cpp.
void CParticleFilter::initializeUniform | ( | mcl::pose | Pmin, |
mcl::pose | Pmax, | ||
mcl::pose | dP | ||
) |
Initializes the distribution using uniform distribution The box Pmin and Pmax sets the borders and dP sets the step size Pmin
the minimum values Pmax
maximum values dP
step size
< number of particles for each component
If something stupid should happen above
Definition at line 182 of file CParticleFilter.cpp.
void CParticleFilter::myfree | ( | ) |
Frees the reserved memory
Definition at line 48 of file CParticleFilter.cpp.
void CParticleFilter::normalize | ( | ) |
Performs the normalization step i.e. according to updated likelyhoods the probability of each particle is calculated and the whole distribution gets probablity of 1
Definition at line 303 of file CParticleFilter.cpp.
void CParticleFilter::predict | ( | mcl::pose | dP, |
mcl::pose | std | ||
) |
Performs the prediction step Moves the cloud according to estimated movement() and adds noise according to Assumes independed noise (i.e the x,y,a are not correlated, which is not really true)
dP | relative movement between the scans |
std | The standard deviation of the noise that is independently added to each component |
Modified 25.9.2008 to induce the noise into the differential movement instead of into the trajectory in world frame
<estimates
Generate noise from normal distribution
Definition at line 330 of file CParticleFilter.cpp.
void CParticleFilter::predict | ( | mcl::pose | dP, |
float | Q[4] | ||
) |
Performs prediction step with system noise covariance matrix Moves the particles according to and samples the noise Note that the angle noise is not correlated with the position using covariance matrix [4] dP
relative motion between steps Q
[4] Uncertainty as covariance (Var(X) Var(XY) Var(Y) STD(A)!!)
Performs prediction step with system noise covariance matrix Moves the particles according to and samples the noise Note that the angle noise is not correlated with the position using covariance matrix [4] dP
relative motion between steps Q
[4] Uncertainty as covariance (Var(X) Var(XY) Var(Y) STD(A)!!) FIXME: NOT TESTED
Transform the correlated noise according to the heading of the particle FIXME: Remember to test the validity as this is not tested
Definition at line 371 of file CParticleFilter.cpp.
void CParticleFilter::print | ( | ) |
Prints some status data
Definition at line 480 of file CParticleFilter.cpp.
void CParticleFilter::resize | ( | int | n | ) |
Resizes the distribution so that n particles from indices *ind will be saved NOTE:: The memory will not be reallocated
n
the size of *ind and the "new" vector
Resizes the distribution so that n particles from indices *ind will be saved NOTE:: The memory will not be reallocated n
the size of the "new" vector
< find indices for the best ones
Definition at line 495 of file CParticleFilter.cpp.
void CParticleFilter::saveToFile | ( | int | Fileind | ) |
Definition at line 616 of file CParticleFilter.cpp.
void CParticleFilter::SIRUpdate | ( | ) |
Performs the Sample Importance Resampling (SIR) algorithm for the distribution The algorithm chooses the best particles (with respect to the probability) and resamples these.
You should have updated the likelihoods and normalized the distribution before running this Also, it might be smart not to run this in every iteration, since the distribution looses accuracy due to the "discretation"
j++; ///WAS HERE until 30.7.2008
Definition at line 226 of file CParticleFilter.cpp.
void CParticleFilter::updateLikelihood | ( | float * | lik | ) |
You Can use this to update the vector of likelihoods Most probably you will use this update step so that you give the whole filter to some likelihood function, which updates the public data TPoseParticle *Particles directly however, this is here if you like to use it :)
*lik | the vector of likelihoods for the particles (has to be the same size as the number of particles) |
Definition at line 469 of file CParticleFilter.cpp.
Distribution mean.
Definition at line 365 of file CParticleFilter.h.
Tells if the distribution mean has been calculated.
Definition at line 367 of file CParticleFilter.h.
Likelihood of the distribution.
Definition at line 361 of file CParticleFilter.h.
The random number class.
Definition at line 368 of file CParticleFilter.h.
Number of particles in distribution.
Definition at line 363 of file CParticleFilter.h.
Percentage of the outlier measurements in distribution.
Definition at line 362 of file CParticleFilter.h.
Particle distribution.
Definition at line 360 of file CParticleFilter.h.
reserved memory Size (number of reserved particles!)
Definition at line 364 of file CParticleFilter.h.
TPoseParticle* mcl::CParticleFilter::tmp [private] |
Particle distribution for SIR (Maintained for efficiency reasons here)
Definition at line 481 of file CParticleFilter.h.
Distribution variance.
Definition at line 366 of file CParticleFilter.h.