particleFilter.hpp

/* $Id$ */

#ifndef FILTER_PARTICLE_FILTER_HPP
#define FILTER_PARTICLE_FILTER_HPP

#include "jmath/jblas.hpp"
#include "jmath/random.hpp"

#include "filter/predictModel.hpp"
#include "filter/observeModel.hpp"

namespace jafar {
  namespace filter {


    class ParticleFilter {
    public:

//       typedef unsigned int syze_type;

      typedef std::vector<jblas::vec>::iterator STATE_IT;
      typedef std::vector<jblas::vec>::const_iterator STATE_CONST_IT;

      //      ParticleFilter(std::size_t nbParticles_, std::size_t sizeState_);

//       /** Create a particle filter with a set of identical particles.
//        * @param nbParticles_ The initial number of particles
//        * @param initState_ The initial state of the particles
//        */
//       ParticleFilter(std::size_t nbParticles_, const vec& initState_);

      ParticleFilter(std::size_t nbParticles_, std::size_t sizeState_, 
                     const jblas::vec& stateMin_, const jblas::vec& stateMax_);

      ~ParticleFilter();

      inline std::size_t sizeState() const {return _sizeState;};
      inline std::size_t getNbParticles() const {return nbParticles;};

      double getWeight(std::size_t index_) const;
      const jblas::vec& getState(std::size_t index_) const;

      void predict(LinearPredictModel& m_);

      //void update(jafar::filter::JacobianObserveModel& m_, const jblas::vec& z_, const jblas::vec& R_);
      void update(jafar::filter::JacobianObserveModel& m_, double z_);

//       /** Resample the particles. The number of particles remains the
//        * same.
//        */
//       void resample();

//       /** Resample a new nember of particles.
//        * @param nbParticles_ The new nember of particles
//        */
//       void resample(int nbParticles_);

      const jblas::vec& getStateMean();
      
      const jblas::sym_mat& getStateCov();

    protected:

      std::size_t nbParticles;
      std::size_t _sizeState;

      jblas::vec weight;
      std::vector<jblas::vec> states;

      jblas::vec stateMean;
      jblas::sym_mat stateCov;
      bool meanAndCovUpToDate;

      jafar::jmath::MultiDimNormalDistribution predictNoise;

      void computeMeanAndCov();
      
    };

  } // namespace filter
} // namespace jafar

#endif // FILTER_PARTICLE_FILTER_HPP