Public Member Functions | Public Attributes
ParticleFilter3D Class Reference

#include <ParticleFilter3D.h>

List of all members.

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< PoseParticlepcloud
 Particle distribution.

Detailed Description

This is an implementation for 3D (or 6D to be more precise) particle filter.

Author:
Jari Saarinen (jari.p.saarinen@gmail.com)

Definition at line 19 of file ParticleFilter3D.h.


Constructor & Destructor Documentation

Definition at line 24 of file ParticleFilter3D.h.


Member Function Documentation

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.

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.

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 ( ) [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.


Member Data Documentation

The random number class.

Definition at line 22 of file ParticleFilter3D.h.

Particle distribution.

Definition at line 21 of file ParticleFilter3D.h.


The documentation for this class was generated from the following files:


ndt_mcl
Author(s): jari
autogenerated on Mon Jan 6 2014 11:32:07