particlefilter.hpp

dysii是一款非常出色的滤波函数库

#ifndef INDII_ML_FILTER_PARTICLEFILTER_HPP
#define INDII_ML_FILTER_PARTICLEFILTER_HPP

#include "../aux/DiracMixturePdf.hpp"
#include "../aux/Parallel.hpp"  

#include "Filter.hpp"
#include "ParticleResampler.hpp"
#include "ParticleFilterModel.hpp"
#include "DeterministicParticleResampler.hpp"

namespace indii {
  namespace ml {
    namespace filter {

/**
 * Particle %filter.
 *
 * @author Lawrence Murray <[e-mail]>
 * @version $Rev: 388 $
 * @date $Date: 2008-02-15 22:28:28 +0000 (Fri, 15 Feb 2008) $
 *
 * @param T The type of time.
 *
 * ParticleFilter is suitable for systems with nonlinear transition
 * and measurement functions, approximating state and noise with
 * indii::ml::aux::DiracMixturePdf distributions.
 * 
 * @see indii::ml::filter for general usage guidelines.
 *
 * @section ParticleFilter_details Details
 *
 * Rather than assume that the state at any time is Gaussian
 * distributed, as in the case of KalmanFilter and similar, arbitrary
 * distributions may be admitted by approximating the state with
 * indii::ml::aux::DiracMixturePdf. This leads to a family of MCMC
 * importance sampling methods commonly called <i>particle
 * filters</i>. The particular implementation here is based on
 * <i>sequential importance sampling</i>, and in particular the @em
 * Condensation algorithm @ref Isard1998a "(Isard & Blake 1998)".
 *
 * To estimate the filtered density at time \f$t_n\f$ this proceeds as
 * follows:
 *
 * @li Propagate each of the particles
 * \f$\mathbf{s}_{t_{n-1}}^{(i)}\f$ from the previous time
 * \f$t_{n-1}\f$ through the state transition function \f$f\f$ to
 * obtain the weighted particle set \f$\{\mathbf{s}_{t_n}^{(i)} =
 * f(\mathbf{s}_{t_{n-1}}^{(i)},\mathbf{v}^{(i)}),\pi_{t_{n-1}}^{(i)})\}\f$
 * (for particular noise samples \f$\mathbf{v}^{(i)}\f$), representing
 * the proposal distribution
 * \f$P(\mathbf{x}(t_n)\,|\,\mathbf{y}(t_1),\ldots,\mathbf{y}(t_{n-1}))\f$.
 *
 * @li Update weights using the measurement function \f$g\f$, calculating
 * the density of the actual measurement \f$\mathbf{y}(t_n)\f$ within
 * \f$g(\mathbf{s}_{t_n}^{(i)},\mathbf{w})\f$ for each particle and
 * normalising across all weights to obtain the updated set
 * \f$\{\mathbf{s}_{t_n}^{(i)},\pi_{t_n}^{(i)})\}\f$, representing the
 * desired distribution
 * \f$P(\mathbf{x}(t_n)\,|\,\mathbf{y}(t_1),\ldots,\mathbf{y}(t_n))\f$.
 *
 * The main problem with this approach is that particles tend to fall
 * off the likelihood mass over time so that very few have any
 * significant weight and the distribution is poorly represented. This
 * is referred to as @em degeneracy. The brute force approach is to
 * simply increase \f$P\f$, adding more particles. A cleverer approach
 * is to use a @em resampling technique that eliminates particles with
 * low weight, while multiplying particles with high weight.
 *
 * The resampling step is inserted before the first step above to
 * obtain a resampled set of particles to propagate through the state
 * transition function and form the proposal distribution. A number of
 * resampling strategies exist, which are provided by the
 * ParticleResampler classes.
 *
 * @section ParticleFilter_references References
 *
 * @anchor Isard1998a Isard, M. & Blake, A. Condensation --
 * Conditional Density Propagation for Visual
 * Tracking. <i>International Journal of Computer Vision</i>,
 * <b>1998</b>, <i>29</i>, 5-28.
 */
template<class T = unsigned int>
class ParticleFilter : public Filter<T,indii::ml::aux::DiracMixturePdf> {
public:
  /**
   * Create new particle %filter.
   *
   * @param model Model to estimate.
   * @param p_x0 \f$P\big(\mathbf{x}(0)\big)\f$; prior over the
   * initial state \f$\mathbf{x}(0)\f$.
   */
  ParticleFilter(ParticleFilterModel<T>* model,
      indii::ml::aux::DiracMixturePdf& p_x0);

  /**
   * Destructor.
   */
  virtual ~ParticleFilter();

  /**
   * Get the model being estimated.
   *
   * @return The model being estimated.
   */
  virtual ParticleFilterModel<T>* getModel();

  virtual void filter(T tnp1, const indii::ml::aux::vector& ytnp1);
  
  virtual indii::ml::aux::DiracMixturePdf measure();

  /**
   * Resample the filtered state.
   *
   * @param resampler A particle resampler.
   *
   * Resamples the filtered state using the given
   * ParticleResampler. Zero or more resample() calls may be made
   * between each call of filter() to resample the filtered state as
   * desired.
   *
   * Calling this method and passing in the resampler (visitor
   * pattern) is preferred to separate calls to getFilteredState(),
   * ParticleResampler::resample() and setFilteredState().
   */
  void resample(ParticleResampler* resampler);

private:
  /**
   * Model.
   */
  ParticleFilterModel<T>* model;

};

    }
  }
}

#include "boost/mpi.hpp"
#include "boost/mpi/environment.hpp"
#include "boost/mpi/communicator.hpp"

#include <assert.h>
#include <vector>

namespace aux = indii::ml::aux;

using namespace indii::ml::filter;

template <class T>
ParticleFilter<T>::ParticleFilter(ParticleFilterModel<T>* model,
    aux::DiracMixturePdf& p_x0) :
    Filter<T,aux::DiracMixturePdf>(p_x0), model(model) {
  //
}

template <class T>
ParticleFilter<T>::~ParticleFilter() {
  //
}

template <class T>
ParticleFilterModel<T>* ParticleFilter<T>::getModel() {
  return model;
}

template <class T>
void ParticleFilter<T>::filter(T tnp1, const aux::vector& ytnp1) {
  /* references to simplify code */
  aux::DiracMixturePdf& p_xtn_ytn = Filter<T,aux::DiracMixturePdf>::p_xtn_ytn;
  T& tn = Filter<T,aux::DiracMixturePdf>::tn;

  /* pre-condition */
  assert (tnp1 >= tn);

  aux::DiracMixturePdf p_xtnp1_ytnp1(model->getStateSize());
  aux::vector x(model->getStateSize());
  double w;
  T delta = tnp1 - tn;
  aux::DiracMixturePdf::weighted_component_const_iterator iter, end;

  /* filter */
  iter = p_xtn_ytn.getComponents().begin();
  end = p_xtn_ytn.getComponents().end();
  while (iter != end) {
    x = model->transition(iter->x, tn, delta);
    w = iter->w * model->weight(x, ytnp1);

    p_xtnp1_ytnp1.addComponent(x, w);

    iter++;
  }

  /* update state */
  tn = tnp1;
  p_xtn_ytn = p_xtnp1_ytnp1;
}

template <class T>
aux::DiracMixturePdf ParticleFilter<T>::measure() {
  DeterministicParticleResampler resampler;
  aux::DiracMixturePdf resampled(resampler.resample(
      Filter<T,aux::DiracMixturePdf>::getFilteredState()));
  resampled.redistributeByCount();

  aux::DiracMixturePdf p_ytn_xtn(model->getMeasurementSize());
  aux::DiracMixturePdf::weighted_component_const_iterator iter, end;
  iter = resampled.getComponents().begin();
  end = resampled.getComponents().end();
  while (iter != end) {
    p_ytn_xtn.addComponent(model->measure(iter->x));
    iter++;
  }

  return p_ytn_xtn;
}

template <class T>
void ParticleFilter<T>::resample(ParticleResampler* resampler) {
  aux::DiracMixturePdf resampled(resampler->resample(
      Filter<T,aux::DiracMixturePdf>::getFilteredState()));
  Filter<T,aux::DiracMixturePdf>::setFilteredState(resampled);
}

#endif