ParticleFilter.h

Go to the documentation of this file.

/*
 GRT MIT License
 Copyright (c) <2012> <Nicholas Gillian, Media Lab, MIT>
 
 Permission is hereby granted, free of charge, to any person obtaining a copy of this software
 and associated documentation files (the "Software"), to deal in the Software without restriction,
 including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
 and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so,
 subject to the following conditions:
 
 The above copyright notice and this permission notice shall be included in all copies or substantial
 portions of the Software.
 
 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT
 LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
 SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#ifndef GRT_PARTICLE_FILTER_HEADER
#define GRT_PARTICLE_FILTER_HEADER

#include "Particle.h"
#include "../../Util/GRTCommon.h"

GRT_BEGIN_NAMESPACE

template<class PARTICLE,class SENSOR_DATA>
class ParticleFilter{
    
public:
    ParticleFilter():particles(particleDistributionA){
        initialized = false;
        verbose = true;
        normWeights = true;
        initMode = INIT_MODE_UNIFORM;
        estimationMode = WEIGHTED_MEAN;
        numParticles = 0;
        stateVectorSize = 0;
        numDeadParticles = 0;
        wNorm = 0;
        wDotProduct = 0;
        minimumWeightThreshold = 1.0e-20;
        resampleThreshold = 1.0e-5;
        robustMeanWeightDistance = 0.2;
        estimationLikelihood = 0;
        warningLog.setProceedingText("[WARNING ParticleFilter]");
        errorLog.setProceedingText("[ERROR ParticleFilter]");
    }
    
    ParticleFilter(const ParticleFilter &rhs):particles(particleDistributionA){
        *this = rhs;
    }
    
    virtual ~ParticleFilter(){
    }
    
    PARTICLE& operator[](const unsigned int &i){
        return particles[i];
    }
    
    const PARTICLE& operator[](const unsigned int &i) const {
        return particles[i];
    }
    
    PARTICLE& operator()(const unsigned int &i){
        return (&particles == &particleDistributionA ? particleDistributionB[i] : particleDistributionA[i]);
    }
    
    const PARTICLE& operator()(const unsigned int &i) const {
        return (&particles == &particleDistributionA ? particleDistributionB[i] : particleDistributionA[i]);
    }
    
    ParticleFilter& operator=(const ParticleFilter &rhs){
        if( this != &rhs ){
            this->initialized = rhs.initialized;
            this->verbose = rhs.verbose;
            this->normWeights = rhs.normWeights;
            this->numParticles= rhs.numParticles;
            this->stateVectorSize = rhs.stateVectorSize;
            this->initMode = rhs.initMode;
            this->estimationMode= rhs.estimationMode;
            this->numDeadParticles = rhs.numDeadParticles;
            this->wNorm = rhs.wNorm;
            this->wDotProduct = rhs.wDotProduct;
            this->minimumWeightThreshold = rhs.minimumWeightThreshold;
            this->resampleThreshold = rhs.resampleThreshold;
            this->robustMeanWeightDistance= rhs.robustMeanWeightDistance;
            this->estimationLikelihood = rhs.estimationLikelihood;
            this->x = rhs.x;
            this->initModel = rhs.initModel;
            this->processNoise = rhs.processNoise;
            this->measurementNoise = rhs.measurementNoise;
            this->particleDistributionA = rhs.particleDistributionA;
            this->particleDistributionB = rhs.particleDistributionB;
            if( &rhs.particles == &rhs.particleDistributionA ){
                this->particles = particleDistributionA;
            }else{
                this->particles = particleDistributionB;
            }
            this->cumsum = rhs.cumsum;
            this->warningLog = rhs.warningLog;
            this->errorLog = rhs.errorLog;
        }
        return *this;
    }
    
    virtual bool init(const unsigned int numParticles,const Vector< VectorFloat > &initModel,const VectorFloat &processNoise,const VectorFloat &measurementNoise){
        
        //Clear any previous setup
        clear();
        
        //Check to make sure each Vector in the initModel has 2 dimensions, these are min and max or mu and sigma depening on the init mode
        for(unsigned int i=0; i<initModel.size(); i++){
            if( initModel[i].size() != 2 ){
                errorLog << "ERROR: The " << i << " dimension of the initModel does not have 2 dimensions!" << std::endl;
                return false;
            }
        }
        
        stateVectorSize = (unsigned int)initModel.size();
        this->initModel = initModel;
        this->processNoise = processNoise;
        this->measurementNoise = measurementNoise;
        x.resize( stateVectorSize );
        initialized = true;
        
        if( !initParticles( numParticles ) ){
            errorLog << "ERROR: Failed to init particles!" << std::endl;
            clear();
            return false;
        }
        
        return true;
    }
    
    virtual bool initParticles( const UINT numParticles ){
        
        if( !initialized ) return false;
        
        if( stateVectorSize != x.size() ){
            return false;
        }
        
        this->numParticles = numParticles;
        particleDistributionA.clear();
        particleDistributionB.clear();
        cumsum.clear();
        particleDistributionA.resize( numParticles, PARTICLE(stateVectorSize) );
        particleDistributionB.resize( numParticles, PARTICLE(stateVectorSize) );
        particles = particleDistributionA;
        cumsum.resize( numParticles,0 );
        
        reset();
        
        return true;
    }

    
    virtual bool filter(SENSOR_DATA &data){
        
        if( !initialized ){
            errorLog << "ERROR: The particle filter has not been initialized!" << std::endl;
            return false;
        }

        if( !preFilterUpdate( data ) ){
            errorLog << "ERROR: Failed to complete preFilterUpdate!" << std::endl;
            return false;
        }
        
        unsigned int i = 0;
        typename Vector< PARTICLE >::iterator iter;
        
        //The main particle prediction loop
        for( iter = particles.begin(), i = 0; iter != particles.end(); ++iter, i++ ){
            if( !predict( *iter ) ){
                errorLog << "ERROR: Particle " << i << " failed prediction!" << std::endl;
                return false;
            }
        }
        
        //The main particle update loop
        for( iter = particles.begin(), i = 0; iter != particles.end(); ++iter, i++ ){
            if( !update( *iter, data ) ){
                errorLog << "ERROR: Particle " << i << " failed update!" << std::endl;
                return false;
            }
        }
        
        //Normalize the particle weights so they sum to 1
        if( normWeights ){
            if( !normalizeWeights() ){
                errorLog << "ERROR: Failed to normalize particle weights! " << std::endl;
                return false;
            }
        }
        
        //Compute the final state estimate
        if( !computeEstimate() ){
            errorLog << "ERROR: Failed to compute the final estimat!" << std::endl;
            return false;
        }
        
        //Check to see if we should resample the particles for the next iteration
        if( checkForResample() ){
        
            //Resample the particles
            if( !resample() ){
                errorLog << "ERROR: Failed to resample particles!" << std::endl;
                return false;
            }
        }

        if( !postFilterUpdate( data ) ){
            errorLog << "ERROR: Failed to complete postFilterUpdate!" << std::endl;
            return false;
        }
        
        return true;
    }
    
    virtual bool clear(){
        initialized = false;
        numParticles = 0;
        stateVectorSize = 0;
        estimationLikelihood = 0;
        wNorm = 0;
        wDotProduct = 0;
        numDeadParticles = 0;
        x.clear();
        initModel.clear();
        processNoise.clear();
        measurementNoise.clear();
        particleDistributionA.clear();
        particleDistributionB.clear();
        cumsum.clear();
        return true;
    }
    
    virtual bool reset(){
        
        if( !initialized ){
            return false;
        }
        
        for(unsigned int i=0; i<numParticles; i++){
            for(unsigned int j=0; j<stateVectorSize; j++){
                switch( initMode ){
                    case INIT_MODE_UNIFORM:
                        particles[i].x[j] = rand.getRandomNumberUniform(initModel[j][0],initModel[j][1]);
                        break;
                    case INIT_MODE_GAUSSIAN:
                        particles[i].x[j] = initModel[j][0] + rand.getRandomNumberGauss(0,initModel[j][1]);
                        break;
                    default:
                        errorLog << "ERROR: Unknown initMode!" << std::endl;
                        return false;
                        break;
                }
                
            }
        }
        
        return true;
    }
    
    bool getInitialized() const {
        return initialized;
    }
    
    unsigned int getNumParticles() const{
        return initialized ? (unsigned int)particles.size() : 0;
    }
    
    unsigned int getStateVectorSize() const{
        return initialized ? stateVectorSize : 0;
    }
    
    unsigned int getNumDeadParticles() const{
        return numDeadParticles;
    }
    
    unsigned int getInitMode() const{
        return initMode;
    }
    
    unsigned int getEstimationMode() const{
        return estimationMode;
    }
    
    Float getEstimationLikelihood() const{
        return estimationLikelihood;
    }
    
    Float getWeightSum() const {
        return wNorm;
    }
    
    VectorFloat getStateEstimation() const{
        return x;
    }
    
    Vector< PARTICLE > getParticles(){
        return (&particles == &particleDistributionA ? particleDistributionA : particleDistributionB);
    }
    
    Vector< PARTICLE > getOldParticles(){
        return (&particles == &particleDistributionA ? particleDistributionB : particleDistributionA);
    }
    
    VectorFloat setProcessNoise() const{
        return processNoise;
    }
    
    bool setVerbose(const bool verbose){
        this->verbose = verbose;
        return true;
    }
    
    bool setNormalizeWeights(const bool normWeights){
        this->normWeights = normWeights;
        return true;
    }
    
    bool setResampleThreshold(const Float resampleThreshold){
        this->resampleThreshold = resampleThreshold;
        return true;
    }
    
    bool setEstimationMode(const unsigned int estimationMode){
        if( estimationMode == MEAN || estimationMode == WEIGHTED_MEAN ||
            estimationMode == ROBUST_MEAN || estimationMode == BEST_PARTICLE )
        {
            this->estimationMode = estimationMode;
            return true;
        }
        return false;
    }
    
    bool setInitMode(const unsigned int initMode){
        if( initMode == INIT_MODE_UNIFORM || initMode == INIT_MODE_GAUSSIAN )
        {
            this->initMode = initMode;
            return true;
        }
        return false;
    }
    
    bool setInitModel(const Vector< VectorFloat > initModel){
        if( this->initModel.size() == initModel.size() ){
            this->initModel = initModel;
            return true;
        }
        return false;
    }
    
    bool setProcessNoise(const VectorFloat &processNoise){
        if( this->processNoise.size() == processNoise.size() ){
            this->processNoise = processNoise;
            return true;
        }
        return false;
    }
    
    bool setMeasurementNoise(const VectorFloat &measurementNoise){
        if( this->measurementNoise.size() == measurementNoise.size() ){
            this->measurementNoise = measurementNoise;
            return true;
        }
        return false;
    }
    
protected:
    virtual bool predict( PARTICLE &p ){
        errorLog << "predict( PARTICLE &p ) Prediction function not implemented! This must be implemented by the derived class!" << std::endl;
        return false;
    }
    
    virtual bool update( PARTICLE &p, SENSOR_DATA &data ){
        errorLog << "update( PARTICLE &p, SENSOR_DATA &data ) Update function not implemented! This must be implemented by the derived class!" << std::endl;
        return false;
    }
    
    virtual bool normalizeWeights(){
        
        //Compute the total particle weight and number of dead particles
        wNorm = 0;
        wDotProduct = 0;
        numDeadParticles = 0;
        typename Vector< PARTICLE >::iterator iter;
        for( iter = particles.begin(); iter != particles.end(); ++iter ){
            if( grt_isinf( iter->w ) ){
                numDeadParticles++;
                iter->w = 0;
            }else{
                wNorm += iter->w;
            }
        }
        
        if( wNorm == 0 ){
            if( verbose )
                warningLog << "normalizeWeights() - Weight norm is zero!" << std::endl;
            return true;
        }
        
        //Normalized the weights so they sum to 1
        Float weightUpdate = 1.0 / wNorm;
        for( iter = particles.begin(); iter != particles.end(); ++iter ){
            
            //Normalize the weights (so they sum to 1)
            iter->w *= weightUpdate;
            
            //Compute the total weights dot product (this is used later to test for degeneracy)
            wDotProduct += iter->w * iter->w;
        }
        wDotProduct = 1.0 / wDotProduct;
        
        //cout << "wNorm: " << wNorm << " wDotProduct: " << wDotProduct << std::endl;

        return true;
    }
    
    virtual bool computeEstimate(){
        
        typename Vector< PARTICLE >::iterator iter;
        const unsigned int N = (unsigned int)x.size();
        unsigned int bestIndex = 0;
        unsigned int robustMeanParticleCounter = 0;
        Float bestWeight = 0;
        Float sum = 0;
        estimationLikelihood = 0;
        switch( estimationMode ){
            case MEAN:
                for(unsigned int j=0; j<N; j++){
                    x[j] = 0;
                }
                
                for( iter = particles.begin(); iter != particles.end(); ++iter ){
                    for(unsigned int j=0; j<N; j++){
                        x[j] += iter->x[j];
                    }
                    estimationLikelihood += grt_isnan(iter->w) ? 0 : iter->w;
                }
                
                for(unsigned int j=0; j<N; j++){
                    x[j] /= Float(numParticles);
                }
                estimationLikelihood /= Float(numParticles);
                break;
            case WEIGHTED_MEAN:
                for(unsigned int j=0; j<N; j++){
                    x[j] = 0;
                    sum = 0;
                    for( iter = particles.begin(); iter != particles.end(); ++iter ){
                        x[j] += iter->x[j] * iter->w;
                        sum += iter->w;
                    }
                    x[j] /= sum;
                }
                
                for( iter = particles.begin(); iter != particles.end(); ++iter ){
                    estimationLikelihood += grt_isnan(iter->w) ? 0 : iter->w;
                }
                estimationLikelihood /= Float(numParticles);
                break;
            case ROBUST_MEAN:
                //Reset x
                for(unsigned int j=0; j<N; j++){
                    x[j] = 0;
                }
                sum = 0;
                
                //Find the particle with the best weight
                for(unsigned int i=0; i<numParticles; i++){
                    if( particles[i].w > bestWeight ){
                        bestWeight = particles[i].w;
                        bestIndex = i;
                    }
                }
                
                //Use all the particles within a given distance of that weight
                for( iter = particles.begin(); iter != particles.end(); ++iter ){
                    if( fabs( iter->w - particles[bestIndex].w ) <= robustMeanWeightDistance ){
                        for(unsigned int j=0; j<N; j++){
                            x[j] += iter->x[j] * iter->w;
                        }
                        estimationLikelihood += grt_isnan(iter->w) ? 0 : iter->w;
                        sum += iter->w;
                        robustMeanParticleCounter++;
                    }
                }
                
                //Normalize x
                for(unsigned int j=0; j<N; j++){
                    x[j] /= sum;
                }
                estimationLikelihood /= Float(robustMeanParticleCounter);
                break;
            case BEST_PARTICLE:
                for(unsigned int i=0; i<numParticles; i++){
                    if( particles[i].w > bestWeight ){
                        bestWeight = particles[i].w;
                        bestIndex = i;
                    }
                }
                x = particles[bestIndex].x;
                estimationLikelihood = grt_isnan(particles[bestIndex].w) ? 0 : particles[bestIndex].w;
                break;
            default:
                errorLog << "ERROR: Unknown estimation mode!" << std::endl;
                return false;
                break;
        }
        return true;
    }
    
    virtual bool checkForResample(){
        return wNorm < resampleThreshold;
    }
    
    virtual bool resample(){
        
        Vector< PARTICLE > *tempParticles = NULL;
        
        //If the particles are currently reference the first particle distribution, then set the tempParticles to the second distribution
        if( &particles == &particleDistributionA ) tempParticles = &particleDistributionB;
        else tempParticles = &particleDistributionA;
        
        //Select any weight that is above the minimum weight threshold
        Vector< IndexedDouble > weights;
        weights.reserve(numParticles);
        for(unsigned int i=0; i<numParticles; i++){
            if( particles[i].w >= minimumWeightThreshold ){
                weights.push_back( IndexedDouble(i,particles[i].w) );
            }
        }
        
        //Sort the weights
        sort(weights.begin(),weights.end(),IndexedDouble::sortIndexedDoubleByValueAscending);
        const unsigned int numWeights = (unsigned int)weights.size();
        unsigned int randIndex = 0;
        
        //If there are no valid weights then we just pick N random particles
        if( numWeights == 0 ){
            for(unsigned int n=0; n<numParticles; n++){
                (*tempParticles)[n] = particles[ rand.getRandomNumberInt(0, numParticles) ];
            }
            if( &particles == &particleDistributionA ) particles = particleDistributionB;
            else particles = particleDistributionA;
            return true;
        }
        
        //Compute the cumulative sum
        cumsum[0] = weights[0].value;
        for(unsigned int i=1; i<numWeights; i++){
            cumsum[i] = cumsum[i-1] + weights[i].value;
        }
        
        //Resample the weights
        const unsigned int numRandomParticles = (unsigned int) round(numParticles/100.0*10.0);
        for(unsigned int n=0; n<numParticles; n++){
            
            if( numParticles-n > numParticles-numRandomParticles){
            
                //Pick a random number between 0 and the max cumsum
                Float randValue = rand.getRandomNumberUniform(0,cumsum[numWeights-1]);
                randIndex = 0;
                
                //Find which bin the rand value falls into, set the random index to this value
                for(unsigned int i=0; i<numWeights; i++){
                    if( randValue <= cumsum[i] ){
                        randIndex = i;
                        break;
                    }
                }
                
                (*tempParticles)[n] = particles[ weights[randIndex].index ];
            }else{
                //Randomly initalize the weakest particles
                PARTICLE &p = (*tempParticles)[n];
                for(unsigned int j=0; j<stateVectorSize; j++){
                    switch( initMode ){
                        case INIT_MODE_UNIFORM:
                            p.x[j] = rand.getRandomNumberUniform(initModel[j][0],initModel[j][1]);
                            break;
                        case INIT_MODE_GAUSSIAN:
                            p.x[j] = initModel[j][0] + rand.getRandomNumberGauss(0,initModel[j][1]);
                            break;
                        default:
                            errorLog << "ERROR: Unknown initMode!" << std::endl;
                            return false;
                            break;
                    }
                    
                }
            }
        }
        
        //Swap the particle references
        if( &particles == &particleDistributionA ) particles = particleDistributionB;
        else particles = particleDistributionA;
        
        return true;
    }

    virtual bool preFilterUpdate( SENSOR_DATA &data ){
        return true;
    }

    virtual bool postFilterUpdate( SENSOR_DATA &data ){
        return true;
    }
    
    Float gauss(Float x,Float mu,Float sigma){
        return 1.0/(SQRT_TWO_PI*sigma) * exp( -SQR(x-mu)/(2.0*SQR(sigma)) );
    }
    
    Float rbf(const Float x,const Float mu,Float sigma,Float weight=1.0){
        return weight * exp( -SQR( fabs(x-mu) / sigma ) );
    }
    
    Float rbf(const VectorFloat &x,const VectorFloat &mu,Float sigma,Float weight=1.0){
        Float sum = 0;
        const unsigned int N = (unsigned int)x.size();
        for(UINT i=0; i<N; i++){
            sum += fabs(x[i]-mu[i]);
        }
        return weight * exp( -SQR(sum/sigma) );
    }
    
    Float SQR(const Float x){ return x*x; }

    bool initialized;                               
    bool verbose;                                   
    bool normWeights;                               
    unsigned int numParticles;                      
    unsigned int stateVectorSize;                   
    unsigned int initMode;                          
    unsigned int estimationMode;                    
    unsigned int numDeadParticles;
    Float minimumWeightThreshold;                  
    Float robustMeanWeightDistance;                
    Float estimationLikelihood;                    
    Float wNorm;                                   
    Float wDotProduct;                             
    Float resampleThreshold;                       
    VectorFloat x;                                 
    Vector< VectorFloat > initModel;           
    VectorFloat processNoise;                      
    VectorFloat measurementNoise;                  
    Vector< PARTICLE > &particles;              
    Vector< PARTICLE > particleDistributionA;   
    Vector< PARTICLE > particleDistributionB;   
    VectorFloat cumsum;                            
    Random rand;                                    
    WarningLog warningLog;
    ErrorLog errorLog;
    
public:
    enum InitModes{INIT_MODE_UNIFORM=0,INIT_MODE_GAUSSIAN};
    enum EstimationModes{MEAN=0,WEIGHTED_MEAN,ROBUST_MEAN,BEST_PARTICLE};
    
};

GRT_END_NAMESPACE

#endif //GRT_PARTICLE_FILTER_HEADER