particlesmoother.hpp

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

#ifndef INDII_ML_FILTER_PARTICLESMOOTHER_HPP
#define INDII_ML_FILTER_PARTICLESMOOTHER_HPP

#include "Smoother.hpp"
#include "ParticleFilter.hpp"
#include "ParticleSmootherModel.hpp"

namespace indii {
  namespace ml {
    namespace filter {

/**
 * Two-pass particle smoother.
 *
 * @author Lawrence Murray <[e-mail]>
 * @version $Rev: 364 $
 * @date $Date: 2007-12-08 03:18:43 +0000 (Sat, 08 Dec 2007) $
 *
 * ParticleSmoother is suitable for models with nonlinear transition
 * and measurement functions, approximating state and noise with
 * indii::ml::aux::DiracMixturePdf distributions. It is particularly
 * suitable in situations where an appropriate backwards transition
 * function cannot be defined, such as for transition functions
 * defined as convergent differential equations that become divergent
 * when reversed.
 * 
 * @see indii::ml::filter for general usage guidelines.
 *
 * @section ParticleSmoother_details Details
 *
 * The implementation here is of the second algorithm described in
 * @ref Isard1998 "Isard & Blake (1998)". This reweights particles
 * obtained during the forwards pass rather than performing a
 * backwards filtering pass to obtain new particles. While this means
 * a backwards transition function need not be defined for the model,
 * the drawback is that the approach is computationally and spatially
 * expensive -- \f$O(P^2)\f$ in the number of particles \f$P\f$ in
 * both cases.
 *
 * The forwards pass provides a weighted sample set
 * \f$\{(\mathbf{s}_t^{(i)},\pi_t^{(i)})\}\f$ at each time point \f$t =
 * t_1,\ldots,t_T\f$ for \f$i = 1,\ldots,P\f$. Initialising with \f$\psi_{t_T}
 * = \pi_{t_T}\f$, the backwards step to calculate weights at time \f$t_n\f$
 * is as follows:
 *
 * \f{eqnarray*}
 * \alpha_{t_n}^{(i,j)} &=& p(\mathbf{x}({t_{n+1}}) =
 * \mathbf{s}_{t_{n+1}}^{(i)}\,|\,\mathbf{x}({t_n}) =
 * \mathbf{s}_{t_n}^{(j)}) \\
 * \mathbf{\gamma}_{t_n} &=& \alpha_{t_n} \mathbf{\pi}_{t_n} \\
 * \mathbf{\delta}_{t_n} &=& \alpha_{t_n}^T(\mathbf{\psi}_{t_{n+1}}
 * \oslash \mathbf{\gamma}_{t_n})\\
 * \mathbf{\psi}_{t_n} &=& \mathbf{\pi}_{t_n} \otimes
 * \mathbf{\delta}_{t_n}
 * \f}
 *
 * where \f$\oslash\f$ is element-wise division and \f$\otimes\f$
 * element-wise multiplication.
 *
 * These are then normalised so that \f$\sum \psi_{t_n}^{(i)} = 1\f$
 * and the smoothed result
 * \f$\{(\mathbf{s}_{t_n}^{(i)},\psi_{t_n}^{(i)})\}\f$ for \f$i =
 * 1,\ldots,P\f$ is stored.
 *
 * This implementation performs calculations in the above matrix form
 * to take advantage of low-level optimisations in the matrix
 * library. It also uses a sparse matrix implementation of
 * \f$\alpha\f$ to alleviate spatial complexity somewhat.
 *
 * @section ParticleSmoother_references References
 *
 * @anchor Isard1998
 * Isard, M. & Blake, A. A smoothing %filter for
 * Condensation. <i>Proceedings of the 5th European Conference on
 * Computer Vision</i>, <b>1998</b>, 1, 767-781.
 */
template <class T = unsigned int>
class ParticleSmoother : public Smoother<T,indii::ml::aux::DiracMixturePdf>,
    public ParticleFilter<T> {
public:
  /**
   * Create new particle smoother.
   * 
   * @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$.
   */
  ParticleSmoother(ParticleSmootherModel<T>* model,
      indii::ml::aux::DiracMixturePdf& p_x0);

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

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

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

  /**
   * Rewind system to time of previous measurement and
   * smooth.
   *
   * @param tnm1 \f$t_{n-1}\f$; the time to which to rewind the
   * system. This must be less than the current time \f$t_n\f$.
   * @param p_xtnm1_ytnm1 \f$P\big(\mathbf{x}(t_{n-1})\, |
   * \,\mathbf{y}(t_1),\ldots,\mathbf{y}(t_{n-1})\big)\f$;
   * distribution over the state at time \f$t_{n-1}\f$ given past and
   * present measurements (i.e. the estimate from the forward
   * filtering pass at time \f$t_{n-1}\f$).
   */
  virtual void smooth(T tnm1, indii::ml::aux::DiracMixturePdf& p_xtnm1_ytnm1);

  virtual indii::ml::aux::DiracMixturePdf smoothedMeasure();

  /**
   * Resample the smoothed state.
   *
   * @see ParticleFilter::resample()
   */
  void smoothedResample(ParticleResampler* resampler);

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

};

    }
  }
}

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

#include "boost/numeric/ublas/operation.hpp"
#include "boost/numeric/ublas/operation_sparse.hpp"

namespace aux = indii::ml::aux;
namespace ublas = boost::numeric::ublas;

using namespace indii::ml::filter;

template <class T>
ParticleSmoother<T>::ParticleSmoother(ParticleSmootherModel<T>* model,
    aux::DiracMixturePdf& p_x0) :
    Smoother<T,aux::DiracMixturePdf>(p_x0),
    ParticleFilter<T>(model, p_x0), model(model) {
  Smoother<T,aux::DiracMixturePdf>::p_xtn_ytT =
      Filter<T,aux::DiracMixturePdf>::p_xtn_ytn;
}

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

template <class T>
ParticleSmootherModel<T>* ParticleSmoother<T>::getModel() {
  return model;
}

template <class T>
void ParticleSmoother<T>::filter(T tnp1, const aux::vector& ytnp1) {
  ParticleFilter<T>::filter(tnp1, ytnp1);

  /* initialisations for smoothing */
  Smoother<T,aux::DiracMixturePdf>::p_xtn_ytT =
      Filter<T,aux::DiracMixturePdf>::p_xtn_ytn;
}

template <class T>
void ParticleSmoother<T>::smooth(T tnm1, aux::DiracMixturePdf& p_xtnm1_ytnm1) {
  /* references to simplify code */
  aux::DiracMixturePdf& p_xtn_ytn = Filter<T,aux::DiracMixturePdf>::p_xtn_ytn;
  aux::DiracMixturePdf& p_xtn_ytT =
      Smoother<T,aux::DiracMixturePdf>::p_xtn_ytT;
  T& tn = Filter<T,aux::DiracMixturePdf>::tn;

  /* pre-condition */
  assert (tnm1 < tn);

  const unsigned int N = p_xtn_ytn.getDimensions();
  const unsigned int P1 = p_xtnm1_ytnm1.getNumComponents();
  const unsigned int P2 = p_xtn_ytT.getNumComponents();
  const T del = tn - tnm1;
  aux::DiracMixturePdf::weighted_component_const_iterator iter, end;
  unsigned int i;

  boost::mpi::communicator world;
  const int size = world.size();
  int k;

  aux::vector pi(P1);
  aux::vector psi(P2);
  aux::vector gamma(P2);
  aux::vector delta(P1);
  std::vector<aux::sparse_matrix> alphas;

  /* copy forward \f$t_n\f$ filter weights into pi */
  iter = p_xtnm1_ytnm1.getComponents().begin();
  end = p_xtnm1_ytnm1.getComponents().end();
  for (i = 0; iter != end; iter++, i++) {
    pi(i) = iter->w;
  }

  /* copy backward \f$t_{n+1}\f$ smoothed weights into psi */
  iter = p_xtn_ytT.getComponents().begin();
  end = p_xtn_ytT.getComponents().end();
  for (i = 0; iter != end; iter++, i++) {
    psi(i) = iter->w;
  }

  /* calculate \f$\alpha\f$ */
  model->alphaPrecalculate(p_xtnm1_ytnm1, tn, del);
  for (k = 0; k < size; k++) {
    alphas.push_back(model->alpha(p_xtnm1_ytnm1, p_xtn_ytT, tn, del));
    indii::ml::aux::rotate(p_xtn_ytT);
  }

  /* calculate \f$\gamma\f$ */
  gamma.clear();
  for (k = 0; k < size; k++) {
    ublas::axpy_prod(alphas[k], pi, gamma, false);
    indii::ml::aux::rotate(gamma);
  }

  /* calculate \f$\delta\f$ */
  delta.clear();
  psi = element_div(psi, gamma);
  for (k = 0; k < size; k++) {
    ublas::axpy_prod(psi, alphas[k], delta, false);
    indii::ml::aux::rotate(psi);
  }

  /* calculate \f$\psi_{t_{n+1}}\f$ */
  psi.resize(P1);
  noalias(psi) = element_prod(pi, delta);

  /* build smoothed distribution */
  p_xtn_ytT.clear();
  iter = p_xtnm1_ytnm1.getComponents().begin();
  end = p_xtnm1_ytnm1.getComponents().end();
  for (i = 0; iter != end; iter++, i++) {
    p_xtn_ytT.addComponent(iter->x, psi(i));
  }

  /* update state */
  tn = tnm1;
  p_xtn_ytn = p_xtnm1_ytnm1;
}

template <class T>
aux::DiracMixturePdf ParticleSmoother<T>::smoothedMeasure() {
  DeterministicParticleResampler resampler;
  aux::DiracMixturePdf resampled(resampler.resample(
      Smoother<T,aux::DiracMixturePdf>::getSmoothedState()));
  resampled.redistributeByCount();

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

  return p_ytn_xtT;
}

template <class T>
void ParticleSmoother<T>::smoothedResample(ParticleResampler* resampler) {
  aux::DiracMixturePdf resampled(resampler->resample(
      Smoother<T,aux::DiracMixturePdf>::getSmoothedState()));
  Smoother<T,aux::DiracMixturePdf>::setSmoothedState(resampled);
}

#endif