unscentedkalmanfiltermeasurementadaptor.hpp

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

#ifndef INDII_ML_FILTER_UNSCENTEDKALMANFILTERMEASUREMENTADAPTOR_HPP
#define INDII_ML_FILTER_UNSCENTEDKALMANFILTERMEASUREMENTADAPTOR_HPP

#include "UnscentedTransformationModel.hpp"

namespace indii {
  namespace ml {
    namespace filter {

  template <class T> class UnscentedTransformation;
  template <class T> class UnscentedKalmanFilter;

/**
 * Adaptor mapping UnscentedTransformationModel interface to method calls in
 * UnscentedKalmanFilterModel.
 * 
 * @author Lawrence Murray <[e-mail]>
 * @version $Rev: 329 $
 * @date $Date: 2007-10-16 17:10:39 +0100 (Tue, 16 Oct 2007) $
 *
 * @param T The type of time.
 *
 * For internal use only.
 */
template <class T>
class UnscentedKalmanFilterMeasurementAdaptor :
    public UnscentedTransformationModel<T> {

  friend class UnscentedTransformation<T>;
  friend class UnscentedKalmanFilter<T>;

private:
  /**
   * Constructor.
   * 
   * @param model NonLinearModel to which to map calls.
   */
  UnscentedKalmanFilterMeasurementAdaptor(
      const UnscentedKalmanFilter<T>* filter);

  /**
   * Maps call to UnscentedKalmanFilterModel::measure.
   */
  virtual indii::ml::aux::vector propagate(const indii::ml::aux::vector& X,
      T delta = 0);

  /**
   * Model to which to map calls.
   */
  const UnscentedKalmanFilter<T>* filter;

};

    }
  }
}

#include "UnscentedKalmanFilter.hpp"
#include "UnscentedKalmanFilterModel.hpp"

using namespace indii::ml::filter;

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

template <class T>
UnscentedKalmanFilterMeasurementAdaptor<T>::UnscentedKalmanFilterMeasurementAdaptor(const UnscentedKalmanFilter<T>* filter) : filter(filter) {
  //
}

template <class T>
aux::vector UnscentedKalmanFilterMeasurementAdaptor<T>::propagate(
    const aux::vector& X, T delta) {
  const unsigned int N = filter->N;
  const unsigned int W = filter->W;
  const unsigned int V = filter->V;

  aux::vector x(project(X, ublas::range(0,N)));
  aux::vector v(project(X, ublas::range(N+W, N+W+V)));

  return filter->model->measure(x, v);
}

#endif