particle.hpp

Go to the documentation of this file.

#ifndef SSMPACK_FILTER_PARTICLE_HPP
#define SSMPACK_FILTER_PARTICLE_HPP

#include "ssmkit/distribution/conditional.hpp"
#include "ssmkit/filter/recursive_bayesian_base.hpp"
#include "ssmkit/process/hierarchical.hpp"
#include "ssmkit/process/markov.hpp"
#include "ssmkit/process/memoryless.hpp"
#include <armadillo>

#include <tuple>

namespace ssmkit {
namespace filter {

using process::Hierarchical;
using process::Markov;
using process::Memoryless;
using distribution::Conditional;

template <class Process, class Resampler>
class Particle : public RecursiveBayesianBase<Particle<Process, Resampler>> {
 public:
  using CompeleteState = std::tuple<arma::mat, arma::vec>;

 private:
  arma::vec w_;
  arma::mat state_par_;
  Process process_;
  Resampler resampler_;
  unsigned long num_;

 private:
  void normalizeWeights(void) { w_ = w_ / arma::sum(w_); }

 public:
  Particle(Process process, Resampler resampler, unsigned long particles_num)
      : process_{process}, resampler_{resampler}, num_{particles_num} {
    // initialized w_ and state_par_
    w_.resize(num_);
    // take one sample to find out dimension
    auto tmp = process_.template getProcess<0>().getInitialPDF().random();
    state_par_.resize(tmp.size(), num_);
  }
  template <class... Args>
  void predict(const Args &... args) {
    state_par_.each_col([this, &args...](arma::vec &v, const Args &... args) {
      v = process_.template getProcess<0>().getCPDF().random(v, args...);
    });
  }
  template <class Measurement, class... TArgs>
  CompeleteState correct(const Measurement &measurement,
                         const TArgs &... args) {
    unsigned long cnt = 0;
    w_.for_each([this, &cnt, &measurement, &args...](double &e) {
      e *= process_.template getProcess<1>().getCPDF().likelihood(
          measurement, state_par_.col(cnt++), args...);
    });

    normalizeWeights();

    resampler_(state_par_, w_);

    return std::make_tuple(state_par_, w_);
  }
  CompeleteState initialize() {
    state_par_.each_col([this](arma::vec &v) {
      v = process_.template getProcess<0>().getInitialPDF().random();
    });

    unsigned long cnt = 0;
    w_.for_each([this, &cnt](double &e) {
      e = process_.template getProcess<0>().getInitialPDF().likelihood(
          state_par_.col(cnt++));
    });

    normalizeWeights();

    return std::make_tuple(state_par_, w_);
  }
  const arma::vec &getWeights(void) const { return w_; }
  const arma::mat &getStateParticles(void) const { return state_par_; }
};

template <class StatePDF, class StateParamMap, class InitialPDF,
          class MeasurementPDF, class MeasurementParamMap, class Resampler>
auto makeParticle(
    Hierarchical<Markov<StatePDF, StateParamMap, InitialPDF>,
                 Memoryless<MeasurementPDF, MeasurementParamMap>> process,
    Resampler resampler, unsigned long particle_num) {
  return Particle<Hierarchical<Markov<StatePDF, StateParamMap, InitialPDF>,
                               Memoryless<MeasurementPDF, MeasurementParamMap>>,
                  Resampler>(process, resampler, particle_num);
}

} // namespace filter
} // namespace ssmkit

#endif // SSMPACK_FILTER_PARTICLE_HPP