32 #ifndef GRT_PARTICLE_FILTER_HEADER
33 #define GRT_PARTICLE_FILTER_HEADER
36 #include "../../Util/GRTCommon.h"
40 template<
class PARTICLE,
class SENSOR_DATA>
62 warningLog.setProceedingText(
"[WARNING ParticleFilter]");
63 errorLog.setProceedingText(
"[ERROR ParticleFilter]");
95 const PARTICLE&
operator[](
const unsigned int &i)
const {
131 this->numDeadParticles = rhs.numDeadParticles;
150 this->warningLog = rhs.warningLog;
151 this->errorLog = rhs.errorLog;
166 for(
unsigned int i=0; i<initModel.size(); i++){
167 if( initModel[i].size() != 2 ){
168 errorLog <<
"ERROR: The " << i <<
" dimension of the initModel does not have 2 dimensions!" << endl;
181 errorLog <<
"ERROR: Failed to init particles!" << endl;
210 cumsum.resize( numParticles,0 );
227 errorLog <<
"ERROR: The particle filter has not been initialized!" << endl;
232 errorLog <<
"ERROR: Failed to complete preFilterUpdate!" << endl;
237 typename vector< PARTICLE >::iterator iter;
242 errorLog <<
"ERROR: Particle " << i <<
" failed prediction!" << endl;
249 if( !
update( *iter, data ) ){
250 errorLog <<
"ERROR: Particle " << i <<
" failed update!" << endl;
258 errorLog <<
"ERROR: Failed to normalize particle weights! " << endl;
265 errorLog <<
"ERROR: Failed to compute the final estimat!" << endl;
274 errorLog <<
"ERROR: Failed to resample particles!" << endl;
280 errorLog <<
"ERROR: Failed to complete postFilterUpdate!" << endl;
299 numDeadParticles = 0;
310 virtual bool reset(){
319 case INIT_MODE_UNIFORM:
322 case INIT_MODE_GAUSSIAN:
326 errorLog <<
"ERROR: Unknown initMode!" << endl;
368 return numDeadParticles;
486 if( estimationMode == MEAN || estimationMode == WEIGHTED_MEAN ||
487 estimationMode == ROBUST_MEAN || estimationMode == BEST_PARTICLE )
502 if( initMode == INIT_MODE_UNIFORM || initMode == INIT_MODE_GAUSSIAN )
520 if( this->initModel.size() == initModel.size() ){
534 if( this->processNoise.size() == processNoise.size() ){
548 if( this->measurementNoise.size() == measurementNoise.size() ){
563 errorLog <<
"predict( PARTICLE &p ) Prediction function not implemented! This must be implemented by the derived class!" << endl;
577 virtual bool update( PARTICLE &p, SENSOR_DATA &data ){
578 errorLog <<
"update( PARTICLE &p, SENSOR_DATA &data ) Update function not implemented! This must be implemented by the derived class!" << endl;
593 numDeadParticles = 0;
594 typename vector< PARTICLE >::iterator iter;
596 if( grt_isinf( iter->w ) ){
606 warningLog <<
"normalizeWeights() - Weight norm is zero!" << endl;
611 double weightUpdate = 1.0 /
wNorm;
615 iter->w *= weightUpdate;
636 typename vector< PARTICLE >::iterator iter;
637 const unsigned int N = (
unsigned int)
x.size();
638 unsigned int bestIndex = 0;
639 unsigned int robustMeanParticleCounter = 0;
640 double bestWeight = 0;
645 for(
unsigned int j=0; j<N; j++){
650 for(
unsigned int j=0; j<N; j++){
656 for(
unsigned int j=0; j<N; j++){
657 x[j] /= double(numParticles);
662 for(
unsigned int j=0; j<N; j++){
666 x[j] += iter->x[j] * iter->w;
679 for(
unsigned int j=0; j<N; j++){
695 for(
unsigned int j=0; j<N; j++){
696 x[j] += iter->x[j] * iter->w;
700 robustMeanParticleCounter++;
705 for(
unsigned int j=0; j<N; j++){
721 errorLog <<
"ERROR: Unknown estimation mode!" << endl;
746 vector< PARTICLE > *tempParticles = NULL;
753 vector< IndexedDouble > weights;
754 weights.reserve(numParticles);
762 sort(weights.begin(),weights.end(),IndexedDouble::sortIndexedDoubleByValueAscending);
763 const unsigned int numWeights = (
unsigned int)weights.size();
764 unsigned int randIndex = 0;
767 if( numWeights == 0 ){
777 cumsum[0] = weights[0].value;
778 for(
unsigned int i=1; i<numWeights; i++){
783 const unsigned int numRandomParticles = round(numParticles/100.0*10.0);
786 if( numParticles-n > numParticles-numRandomParticles){
793 for(
unsigned int i=0; i<numWeights; i++){
794 if( randValue <=
cumsum[i] ){
800 (*tempParticles)[n] =
particles[ weights[randIndex].index ];
803 PARTICLE &p = (*tempParticles)[n];
806 case INIT_MODE_UNIFORM:
809 case INIT_MODE_GAUSSIAN:
813 errorLog <<
"ERROR: Unknown initMode!" << endl;
855 double gauss(
double x,
double mu,
double sigma){
856 return 1.0/(SQRT_TWO_PI*sigma) * exp( -
SQR(x-mu)/(2.0*
SQR(sigma)) );
870 double rbf(
const double x,
const double mu,
double sigma,
double weight=1.0){
871 return weight * exp( -
SQR( fabs(x-mu) / sigma ) );
885 double rbf(
const VectorDouble &
x,
const VectorDouble &mu,
double sigma,
double weight=1.0){
887 const unsigned int N = (
unsigned int)x.size();
888 for(UINT i=0; i<N; i++){
889 sum += fabs(x[i]-mu[i]);
891 return weight * exp( -
SQR(sum/sigma) );
900 double SQR(
const double x){
return x*
x; }
909 unsigned int numDeadParticles;
929 enum InitModes{INIT_MODE_UNIFORM=0,INIT_MODE_GAUSSIAN};
930 enum EstimationModes{MEAN=0,WEIGHTED_MEAN,ROBUST_MEAN,BEST_PARTICLE};
936 #endif //GRT_PARTICLE_FILTER_HEADER
double getRandomNumberGauss(double mu=0.0, double sigma=1.0)
unsigned int numParticles
The number of particles in the filter.
virtual bool predict(PARTICLE &p)
bool setVerbose(const bool verbose)
bool setInitModel(const vector< VectorDouble > initModel)
double wNorm
Stores the total weight norm value.
PARTICLE & operator[](const unsigned int &i)
std::vector< VectorDouble > initModel
The noise model for the initial starting guess.
VectorDouble getStateEstimation() const
VectorDouble x
The state estimation.
unsigned int stateVectorSize
The size of the state vector (x)
vector< PARTICLE > getParticles()
virtual bool preFilterUpdate(SENSOR_DATA &data)
double resampleThreshold
The threshold below which the particles will be resampled.
bool setResampleThreshold(const double resampleThreshold)
bool setInitMode(const unsigned int initMode)
VectorDouble measurementNoise
The noise covariance in the measurement.
double robustMeanWeightDistance
The distance parameter used in the ROBUST_MEAN estimation mode.
ParticleFilter & operator=(const ParticleFilter &rhs)
unsigned int getNumParticles() const
bool initialized
A flag that indicates if the filter has been initialized.
virtual bool init(const unsigned int numParticles, const vector< VectorDouble > &initModel, const VectorDouble &processNoise, const VectorDouble &measurementNoise)
bool setProcessNoise(const VectorDouble &processNoise)
std::vector< PARTICLE > particleDistributionA
A vector of particles, this will either hold the particles before or after a resample.
const PARTICLE & operator[](const unsigned int &i) const
unsigned int initMode
The mode used to initialize the particles, this should be one of the InitModes enums.
double gauss(double x, double mu, double sigma)
std::vector< PARTICLE > & particles
A reference to the current active particle vector.
virtual bool checkForResample()
virtual bool initParticles(const UINT numParticles)
VectorDouble setProcessNoise() const
double getWeightSum() const
virtual ~ParticleFilter()
VectorDouble processNoise
The noise covariance in the system.
int getRandomNumberInt(int minRange, int maxRange)
unsigned int getNumDeadParticles() const
double getRandomNumberUniform(double minRange=0.0, double maxRange=1.0)
unsigned int getEstimationMode() const
ParticleFilter(const ParticleFilter &rhs)
double minimumWeightThreshold
Any weight below this value will not be resampled.
std::vector< PARTICLE > particleDistributionB
A vector of particles, this will either hold the particles before or after a resample.
vector< PARTICLE > getOldParticles()
const PARTICLE & operator()(const unsigned int &i) const
bool setMeasurementNoise(const VectorDouble &measurementNoise)
virtual bool postFilterUpdate(SENSOR_DATA &data)
unsigned int getStateVectorSize() const
double getEstimationLikelihood() const
virtual bool computeEstimate()
bool getInitialized() const
Random rand
A random number generator.
double SQR(const double x)
double estimationLikelihood
The likelihood of the estimated state.
bool verbose
A flag that indicates if warning and info messages should be printed.
VectorDouble cumsum
The cumulative sum vector used for resampling the particles.
unsigned int getInitMode() const
double wDotProduct
Stores the dot product of all the weights, used to test for degeneracy.
virtual bool update(PARTICLE &p, SENSOR_DATA &data)
bool setNormalizeWeights(const bool normWeights)
double rbf(const VectorDouble &x, const VectorDouble &mu, double sigma, double weight=1.0)
PARTICLE & operator()(const unsigned int &i)
virtual bool filter(SENSOR_DATA &data)
virtual bool normalizeWeights()
bool setEstimationMode(const unsigned int estimationMode)
bool normWeights
A flag that indicates if the weights should be normalized at each filter iteration.
unsigned int estimationMode
The estimation mode (used to compute the state estimation)
double rbf(const double x, const double mu, double sigma, double weight=1.0)