mobilerobotparticlefiltermodel.hpp

来自「dysii是一款非常出色的滤波函数库」· HPP 代码 · 共 60 行

#ifndef MOBILEROBOTPARTICLEFILTERMODEL_HPP
#define MOBILEROBOTPARTICLEFILTERMODEL_HPP

#include "indii/ml/filter/ParticleSmootherModel.hpp"
#include "indii/ml/aux/GaussianPdf.hpp"
#include "indii/ml/aux/vector.hpp"
#include "indii/ml/aux/matrix.hpp"

using namespace indii::ml::filter;

namespace aux = indii::ml::aux;

/**
 * Mobile robot model for particle filter tests.
 */
class MobileRobotParticleFilterModel
    : public ParticleSmootherModel<unsigned int> {
public:
  MobileRobotParticleFilterModel(const aux::GaussianPdf& w,
      const aux::GaussianPdf& v, const aux::GaussianPdf& r);

  virtual ~MobileRobotParticleFilterModel();

  virtual unsigned int getStateSize();

  virtual unsigned int getMeasurementSize();

  virtual aux::vector transition(const aux::vector& x,
      const unsigned int start, const unsigned int delta);

  virtual double weight(const aux::vector& x, const aux::vector& y);

  virtual aux::vector resample(const aux::vector& x);

  virtual aux::vector measure(const aux::vector& x);

  virtual aux::sparse_matrix alpha(const aux::DiracMixturePdf& p_xtn_ytn,
      const aux::DiracMixturePdf& p_xtnp1_ytnp1, const unsigned int start,
      const unsigned int delta);

private:
  /**
   * System noise.
   */
  aux::GaussianPdf w;

  /**
   * Measurement noise.
   */
  aux::GaussianPdf v;

  /**
   * Resampling noise.
   */
  aux::GaussianPdf r;

};

#endif