example/TestParticleFilter.cpp

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/>.
 */

#include "example/ParticleFilter1D.hpp"
#include <vector>
#include <ctime>
#include <cstdlib>
#include <iostream>

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

using namespace Estimation;
using namespace std;

typedef Particle<double,double,double> Particle1Dd;

double randomParticleGenerator(void* v)
{
    return 10.0 * rand_d();
}

int main(int argc, char* argv[])
{
    int nParticles = 25;
    ParticleFilter1D pf(nParticles);
    shared_ptr<ParticleFilter1DBase::CmdModelType>
        cMdl(new PF1DTransCmdModel());
    shared_ptr<ParticleFilter1DBase::MeasModelType>
        mMdl(new PF1DObsModel());
    vector<double> initial_particles;

    pf.setCommandModel(cMdl);
    pf.setMeasurementModel(mMdl);
    pf.setRandomInjectionFunction(randomParticleGenerator);
    pf.setRandomInjection(true);
    
    pf.setFixedRandomInjectionRate(0.1);
    pf.setDynamicRandomInjection(false);
    
    pf.setElitism(true);

    if(pf.getRandomInjection())
        cout << "random injection on!\n";
    else
        exit(0);

    srand(static_cast<unsigned>(time(0)));

    for (int i=0; i<nParticles; i++)
    {
        initial_particles.push_back(randomParticleGenerator(NULL));
    }
    pf.initialize(initial_particles);

    double truePos = 3;
    int nSteps = 100;
    cout << "Starting state: "<<truePos<<endl;
    for (int i=0; i<nSteps; i++)
    {
        double cmd = rand_d() - 0.5; // generate a command
        // "kidnap the robot" with some frequency
        if(i % 50 == 37)
        {
            truePos = 10.0 * rand_d();
            cout << "\nRobot kidnapped to "<<truePos<<"!\n\n";
        }
        truePos = cMdl->updateState(truePos, cmd);    // implement it
        // take a measurement
        double obs = truePos + 0.1*(rand_d() - 0.5);
        // udpate filter
        pf.update(cmd, obs);
        // display estimate and true state
        cout << "Issued command "<<cmd<<", moving to "<<truePos
            <<", and observed "<<obs<<"\n";
        cout << "Mean: "<<pf.getMean()<<", Weighted: "<<pf.getWeightedMean()
            <<", Mode: "<<pf.getMode()<<"\n";
        cout << "Number of particles: "<<pf.getNumParticles()<<"\n";
        cout << endl;
    }

    return 0;
}