Estimation/BayesFilters/ParticleFilter/ParticleFilter.hpp

Go to the documentation of this file.

/*
 *  MAPS-TFSS, Tactile Force Sensor Simulator
 * 
 *  Copyright (C) 2010 Zachary Pezzementi and Gregory Hager
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef ParticleFilter_HPP
#define ParticleFilter_HPP

#include <vector>
#include <ctime>
#include <cstdlib>
#include <stdexcept>
#include <cmath>
#include "Estimation/BayesFilters/BayesFilter.hpp"
#include "boost/shared_ptr.hpp"

#include <iostream>

namespace Estimation
{

    using boost::shared_ptr;

    template<typename CMD, typename STATE>
    class PFCommandModel
    {
    public:
        virtual STATE updateState(const STATE &curState, const CMD &command) const = 0;
    };

    template<typename MEAS, typename STATE>
    class PFMeasurementModel
    {
    public:
        virtual double getLikelihood(const MEAS &m, const STATE &s) = 0;
    };

    template<typename CMD, typename MEAS, typename STATE>
    class Particle
    {
    public:

        // Aliases for associated classes
        typedef PFMeasurementModel<MEAS, STATE> MeasModelType;
        typedef PFCommandModel<CMD, STATE> CmdModelType;

        Particle(const STATE &s) : m_state(s)
        {}

        Particle(const Particle &p) : m_state(p.m_state), m_likelihood(p.m_likelihood)
        {}

        virtual void update(const CMD &c, CmdModelType& mdl)
        {
            m_state = mdl.updateState(m_state, c);
        }

        virtual double computeLikelihood(const MEAS &m, MeasModelType &mdl)
        {
            m_likelihood = mdl.getLikelihood(m, m_state);
            return m_likelihood;
        }

        double getLikelihood() const
        {
            return m_likelihood;
        }

        const STATE& getState() const
        {
            return m_state;
        }

    protected:
        STATE m_state;        
        double m_likelihood;  
    };

    template<typename CMD, typename MEAS, typename STATE>
    class ParticleFilter : public BayesFilter<CMD, MEAS, STATE>
    {
    public:

        // Aliases for subparts
        typedef Particle<CMD, MEAS, STATE> ParticleType;
        typedef PFMeasurementModel<MEAS, STATE> MeasModelType;
        typedef PFCommandModel<CMD, STATE> CmdModelType;
        typedef STATE(*InjectionFnType)(void*);

        ParticleFilter(unsigned nParticles=100, bool injection=false)
                : m_randomInjection(injection), m_injectionFunction(NULL),
                m_injectionArg(NULL),
                m_longTermRate(0.1), m_shortTermRate(0.5),
                m_randomPortion(0.0),
                m_elitism(false),
                m_dynamicInjection(true)
        {
            srand(static_cast<unsigned>(time(0)));
            setNumParticles(nParticles);
        }

        virtual void initialize(const std::vector<STATE> &states);

        inline virtual void CommandUpdate(const CMD &c)
        {
            updateParticles(c);
        }

        virtual void MeasurementUpdate(const MEAS &m);

        inline unsigned getNumParticles() const
        {
            return m_nParticles;
        }

        void setNumParticles(unsigned n)
        {
            if(n == 0)
            {
                throw std::invalid_argument("Number of particles must be at least 1.");
            } else
            m_nParticles = n;
        }

        inline void setMeasurementModel(shared_ptr<MeasModelType> m)
        {
            m_measModel = m;
        }

        inline shared_ptr<MeasModelType> getMeasurementModel() const
        {
            return m_measModel;
        }

        inline void setCommandModel(shared_ptr<CmdModelType> c)
        {
            m_cmdModel = c;
        }

        inline shared_ptr<CmdModelType> getCommandModel() const
        {
            return m_cmdModel;
        }

        inline bool getRandomInjection() const
        {
            return m_randomInjection;
        }

        bool setRandomInjection(bool s);

        inline void setRandomInjectionFunction(InjectionFnType fn,
                                        void* arg=NULL,
                                        bool enable=true)
        {
            m_injectionFunction = fn;
            m_injectionArg = arg;
            setRandomInjection(enable);
        }

        inline InjectionFnType getRandomInjectionFunction() const
        {
            return m_injectionFunction;
        }

        inline void* getRandomInjectionArgument() const
        {
            return m_injectionArg;
        }

        STATE getMode() const;

        inline virtual STATE getCurrentEstimate() const
        {
            return getMode();
        }

        inline bool setFixedRandomInjectionRate(double r)
        {
            if (r > 0.0 && r < 1.0) {
                m_randomPortion = r;
            }
            return m_randomPortion;
        }
        
        inline double getFixedRandomInjectionRate() const
        {
            return m_randomPortion;
        }

        inline void setDynamicRandomInjection(bool enable)
        {
            m_dynamicInjection = enable;
        }

        inline bool getDynamicRandomInjection() const
        {
            return m_dynamicInjection;
        }

        inline double getShortTermDRIRate() const
        {
            return m_shortTermRate;
        }

        inline double getLongTermDRIRate() const
        {
            return m_longTermRate;
        }

        bool setDRIRates(double shortTerm, double longTerm);

        inline void setElitism(bool enable)
        {
            m_elitism = enable;
        }

        inline bool getElitism() const
        {
            return m_elitism;
        }

    protected:
        void updateParticles(const CMD &c);

        void computeLikelihoods(const MEAS &m);

        void importanceSampling(int nParticles);

        virtual void randomInjection(int nParticles);

        double rand_d()
        {
            return static_cast<double>(rand()) / RAND_MAX;
        }

        std::vector<ParticleType>  m_particles;
        std::vector<ParticleType>  m_prevParticles;
        shared_ptr<MeasModelType>               m_measModel;
        shared_ptr<CmdModelType>                m_cmdModel;
        unsigned    m_nParticles;
        double      m_totalLikelihood;

        bool        m_randomInjection;
        STATE     (*m_injectionFunction)(void*);
        void*       m_injectionArg;
        // Helpers for random injection process
        double      m_longTermLikelihood;
        double      m_longTermRate;
        double      m_shortTermLikelihood;
        double      m_shortTermRate;
        double      m_randomPortion;
        bool        m_initMovingAvgs;
        bool        m_elitism;
        bool        m_dynamicInjection;
    };

#include "Estimation/BayesFilters/ParticleFilter/ParticleFilter_impl.hpp"

}

#endif