27 #ifndef GRT_FSM_PARTICLE_FILTER_HEADER
28 #define GRT_FSM_PARTICLE_FILTER_HEADER
30 #include "../../CoreAlgorithms/ParticleFilter/ParticleFilter.h"
49 errorLog <<
"predict( FSMParticle &p ) - Particle Filter has not been initialized!" << endl;
53 if( pt == NULL || pe == NULL ){
54 errorLog <<
"predict( FSMParticle &p ) - pt or pe are NULL!" << endl;
62 const vector< VectorDouble > &model = pe->at( p.currentState );
65 const unsigned int K = (
unsigned int)model.size();
75 errorLog <<
"update( FSMParticle &p, VectorDouble &data ) - Particle Filter has not been initialized!" << endl;
79 if( p.x.size() != data.size() ){
80 errorLog <<
"update( FSMParticle &p, VectorDouble &data ) - x does not match data.size()!" << endl;
86 const size_t N = p.x.size();
87 for(
size_t i=0; i<N; i++){
91 cout <<
"w: " << p.w <<
" p.x: ";
92 for(
size_t i=0; i<N; i++){
93 cout << p.x[i] <<
" ";
96 for(
size_t i=0; i<N; i++){
97 cout << data[i] <<
" ";
104 bool setLookupTables( vector< vector< IndexedDouble > > &pt, vector< vector< VectorDouble > > &pe ){
114 vector< vector< IndexedDouble > > *pt;
115 vector< vector< VectorDouble > > *pe;
virtual bool predict(FSMParticle &p)
VectorDouble measurementNoise
The noise covariance in the measurement.
bool initialized
A flag that indicates if the filter has been initialized.
double gauss(double x, double mu, double sigma)
int getRandomNumberWeighted(const vector< int > &values, const vector< double > &weights)
int getRandomNumberInt(int minRange, int maxRange)
virtual bool update(FSMParticle &p, VectorDouble &data)