mobilerobotunscentedkalmanfiltermodel.cpp

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

#include "MobileRobotUnscentedKalmanFilterModel.hpp"

#define SYSTEM_SIZE 5
#define MEAS_SIZE 1

aux::vector MobileRobotUnscentedKalmanFilterModel::transition(
    const aux::vector& x, const aux::vector& w, unsigned int delta) {
  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;
}

aux::vector MobileRobotUnscentedKalmanFilterModel::measure(
    const aux::vector& x, const aux::vector& v) {
  aux::vector y(MEAS_SIZE);
  y(0) = 2.0 * x(1) + v(0);

  return y;
}

aux::vector MobileRobotUnscentedKalmanFilterModel::backwardTransition(
    const aux::vector& x, const aux::vector& w, unsigned int delta) {
  aux::vector xtnm1(SYSTEM_SIZE);
  xtnm1(0) = x(0) - cos(x(2)) * x(3) - w(0);
  xtnm1(1) = x(1) - sin(x(2)) * x(3) - w(1);
  xtnm1(2) = x(2) - x(4);
  xtnm1(3) = x(3);
  xtnm1(4) = x(4);

  return xtnm1;
}