mobilerobotparticlefiltermodel.cpp

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

#include "MobileRobotParticleFilterModel.hpp"

#define SYSTEM_SIZE 5
#define SYSTEM_NOISE_SIZE 2
#define MEAS_SIZE 1

MobileRobotParticleFilterModel::MobileRobotParticleFilterModel(
    const aux::GaussianPdf& w, const aux::GaussianPdf& v,
    const aux::GaussianPdf& r) : w(w), v(v), r(r) {
  //
}

MobileRobotParticleFilterModel::~MobileRobotParticleFilterModel() {
  //
}

unsigned int MobileRobotParticleFilterModel::getStateSize() {
  return SYSTEM_SIZE;
}

unsigned int MobileRobotParticleFilterModel::getMeasurementSize() {
  return MEAS_SIZE;
}

aux::vector MobileRobotParticleFilterModel::transition(const aux::vector& x,
      const unsigned int start, const unsigned int delta) {
  aux::vector w(SYSTEM_SIZE);
  w = this->w.sample();

  aux::vector xtnp1(SYSTEM_SIZE);
  xtnp1(0) = x(0) + cos(x(2)) * x(3) + w(0);
  xtnp1(1) = x(1) + sin(x(2)) * x(3) + w(1);
  xtnp1(2) = x(2) + x(4);
  xtnp1(3) = x(3);
  xtnp1(4) = x(4);

  return xtnp1;
}

double MobileRobotParticleFilterModel::weight(const aux::vector& x,
    const aux::vector& y) {
  aux::vector mu(MEAS_SIZE);
  mu(0) = 2.0 * x(1);

  return v.calculateDensity(y - mu);
}

aux::vector MobileRobotParticleFilterModel::resample(const aux::vector& x) {
  aux::vector xr(x);
  aux::vector rs(r.sample());
  xr(0) += rs(0);
  xr(1) += rs(1);

  return xr;
}

aux::vector MobileRobotParticleFilterModel::measure(const aux::vector& x) {
  aux::vector y(MEAS_SIZE);
  y(0) = 2.0 * x(1);

  return y + v.sample();
}

aux::sparse_matrix MobileRobotParticleFilterModel::alpha(
    const aux::DiracMixturePdf& p_xtn_ytn,
    const aux::DiracMixturePdf& p_xtnp1_ytnp1,
    const unsigned int start, const unsigned int delta) {
  aux::DiracMixturePdf::weighted_component_const_iterator S1Iter, S1End,
      S2Iter, S2End;
  unsigned int i, j;
  const unsigned int P1 = p_xtn_ytn.getNumComponents();
  const unsigned int P2 = p_xtnp1_ytnp1.getNumComponents();
  double p;
  aux::sparse_matrix alpha(P2,P1);
  aux::vector mu(SYSTEM_NOISE_SIZE), s(SYSTEM_NOISE_SIZE);

  S1Iter = p_xtn_ytn.getComponents().begin();
  S1End = p_xtn_ytn.getComponents().end();
  for (i = 0; S1Iter != S1End; S1Iter++, i++) {
    /* propagate particle through transition to get mean */
    mu(0) = S1Iter->x(0) + cos(S1Iter->x(2)) * S1Iter->x(3);
    mu(1) = S1Iter->x(1) + sin(S1Iter->x(2)) * S1Iter->x(3);

    S2Iter = p_xtnp1_ytnp1.getComponents().begin();
    S2End = p_xtnp1_ytnp1.getComponents().end();
    for (j = 0; S2Iter != S2End; S2Iter++, j++) {
      s(0) = S2Iter->x(0);
      s(1) = S2Iter->x(1);
      p = w.calculateDensity(s - mu);
      if (p > 0.0) {
      	alpha(j,i) = p;
      }
    }
  }

  return alpha;
}