example/ParticleFilter1D.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 ParticleFilter1D_HPP
#define ParticleFilter1D_HPP

#include "Estimation/BayesFilters/ParticleFilter/ParticleFilter.hpp"
#include <cmath>

namespace Estimation
{
    class PF1DTransCmdModel : public PFCommandModel<double, double>
    {
        public:
        double updateState(const double &curState, const double &translation) const
        {
            double randOffset = translation * .2 * (static_cast<double>(rand()) / RAND_MAX - 0.5);
            double newPos = curState + translation + randOffset;
            if(newPos < 0.0) return 0.0;
            if(newPos > 10.0) return 10.0;
            return newPos;
        }
    };

    class PF1DObsModel : public PFMeasurementModel<double, double>
    {
        public:
        PF1DObsModel(double var=0.3) : m_variance(var)
        {}

        double getLikelihood(const double &measurement, const double &stateEst)
        {
            double diff = measurement - stateEst;
            return exp(-(diff * diff) / m_variance);
        }

        double getVariance() const { return m_variance; }

        void setVariance(double v){ m_variance = v; }

        private:
        double m_variance;
    };

    typedef ParticleFilter<double, double, double> ParticleFilter1DBase;

    class ParticleFilter1D : public ParticleFilter1DBase
    {
    public:
        typedef ParticleFilter1DBase::ParticleType Particle1D;

        ParticleFilter1D(unsigned nParticles)
            : ParticleFilter1DBase(nParticles)
        {
        }

        double getMean() const
        {
            std::vector<Particle1D>::const_iterator i;
            double mean = 0.0;
            for (i=m_particles.begin(); i!=m_particles.end(); ++i)
            {
                    mean += i->getState();
            }
            return mean / m_particles.size();
        }

        double getWeightedMean() const
        {
            std::vector<Particle1D>::const_iterator i;
            double mean = 0.0;
            double total = 0.0;
            for (i=m_particles.begin(); i!=m_particles.end(); ++i)
            {
                    mean += i->getState() * i->getLikelihood();
                    total += i->getLikelihood();
            }
            return mean / total;
        }

    };

}

#endif