#include <ParticleFilter3D.h>
Public Member Functions | |
| Eigen::Affine3d | getMean () |
| void | initializeNormalRandom (unsigned int NumParticles, double mx, double my, double mz, double mroll, double mpitch, double myaw, double vx, double vy, double vz, double vroll, double vpitch, double vyaw) |
| void | normalize () |
| ParticleFilter3D () | |
| void | predict (Eigen::Affine3d Tmotion, double vx, double vy, double vz, double vroll, double vpitch, double vyaw) |
| void | SIRUpdate () |
| unsigned int | size () |
| Eigen::Affine3d | xyzrpy2affine (double x, double y, double z, double roll, double pitch, double yaw) |
Public Attributes | |
| ownRandom | myrand |
| The random number class. | |
| std::vector< PoseParticle > | pcloud |
| Particle distribution. | |
This is an implementation for 3D (or 6D to be more precise) particle filter.
Definition at line 19 of file ParticleFilter3D.h.
| ParticleFilter3D::ParticleFilter3D | ( | ) | [inline] |
Definition at line 24 of file ParticleFilter3D.h.
| Eigen::Affine3d ParticleFilter3D::getMean | ( | ) |
Definition at line 127 of file ParticleFilter3D.hpp.
| void ParticleFilter3D::initializeNormalRandom | ( | unsigned int | NumParticles, |
| double | mx, | ||
| double | my, | ||
| double | mz, | ||
| double | mroll, | ||
| double | mpitch, | ||
| double | myaw, | ||
| double | vx, | ||
| double | vy, | ||
| double | vz, | ||
| double | vroll, | ||
| double | vpitch, | ||
| double | vyaw | ||
| ) |
Initializes the filter by sampling from normal distribution with mean in and variance defined by
Initializes the filter using normally distributed random variables with given means (m-values) and standard deviations (v-values)
Definition at line 8 of file ParticleFilter3D.hpp.
| void ParticleFilter3D::normalize | ( | ) |
Performs the normalization step i.e. according to updated likelihoods the probability of each particle is calculated and the whole distribution sums up to 1
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 91 of file ParticleFilter3D.hpp.
| void ParticleFilter3D::predict | ( | Eigen::Affine3d | Tmotion, |
| double | vx, | ||
| double | vy, | ||
| double | vz, | ||
| double | vroll, | ||
| double | vpitch, | ||
| double | vyaw | ||
| ) |
Definition at line 110 of file ParticleFilter3D.hpp.
| void ParticleFilter3D::SIRUpdate | ( | ) |
SIR Update for the filter
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"
<-- Replicate the particle
Check for index error
Leave the loop
Moving on
Index exceeded
Leave the loop
< add the weight to cumulative sum
Make sure that the vector is filled
Definition at line 37 of file ParticleFilter3D.hpp.
| unsigned int ParticleFilter3D::size | ( | void | ) | [inline] |
Definition at line 38 of file ParticleFilter3D.h.
| Eigen::Affine3d ParticleFilter3D::xyzrpy2affine | ( | double | x, |
| double | y, | ||
| double | z, | ||
| double | roll, | ||
| double | pitch, | ||
| double | yaw | ||
| ) | [inline] |
Helper to convert xyzrpy to eigen affine3d
Definition at line 56 of file ParticleFilter3D.h.
The random number class.
Definition at line 22 of file ParticleFilter3D.h.
| std::vector<PoseParticle> ParticleFilter3D::pcloud |
Particle distribution.
Definition at line 21 of file ParticleFilter3D.h.