unscentedkalmanfilter.hpp

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

#ifndef INDII_ML_ESTIMATOR_UNSCENTEDKALMANFILTER_HPP
#define INDII_ML_ESTIMATOR_UNSCENTEDKALMANFILTER_HPP


#include "../aux/vector.hpp"
#include "../aux/matrix.hpp"
#include "../aux/GaussianPdf.hpp"

#include "Filter.hpp"
#include "UnscentedTransformation.hpp"

namespace indii {
  namespace ml {
    namespace filter {

      template <class T> class UnscentedKalmanFilterModel;
      template <class T> class UnscentedKalmanFilterTransitionAdaptor;
      template <class T> class UnscentedKalmanFilterMeasurementAdaptor;

/**
 * Unscented Kalman %filter.
 * 
 * @author Lawrence Murray <[e-mail]>
 * @version $Rev: 344 $
 * @date $Date: 2007-11-02 20:21:05 +0000 (Fri, 02 Nov 2007) $
 *
 * @param T The type of time.
 *
 * UnscentedKalmanFilter is suitable for systems with nonlinear
 * transition and measurement functions, approximating state and noise
 * with indii::ml::aux::GaussianPdf distributions.
 *
 * @see UnscentedTransformation
 * @see indii::ml::filter for general usage guidelines.
 *
 * @section UnscentedKalmanFilter_details Details
 *
 * At time \f$t_n\f$, the filtering step proceeds as follows @ref Wan2000
 * "(Wan & van der Merwe 2000)":
 *
 * @li Approximate
 * \f$P(\mathbf{x}(t_n)\,|\,\mathbf{y}(t_1),\ldots,\mathbf{y}(t_{n-1}))\f$,
 * with mean \f$\hat{\mathbf{x}}\f$ and covariance \f$P_x\f$, by
 * propagating \f$P(\mathbf{x}(t_{n-1}),\mathbf{v}\, |
 * \,\mathbf{y}(t_1),\ldots,\mathbf{y}(t_{n-1}))\f$ through \f$f\f$
 * using UnscentedTransformation.
 *
 * @li Approximate \f$P(\mathbf{y}^*(t_n)\,|\,\mathbf{x}(t_n))\f$,
 * with mean \f$\hat{\mathbf{y}}^*\f$, covariance \f$P_{y^*}\f$ and
 * cross-correlation matrix \f$P_{x y^*}\f$, by propagating
 * \f$P(\mathbf{x}(t_n),\mathbf{w}\, |
 * \,\mathbf{y}(t_1),\ldots,\mathbf{y}(t_{n-1}))\f$ through \f$g\f$
 * using UnscentedTransformation.
 *
 * Now, let the Kalman gain matrix be defined as \f$\mathcal{K} = P_{x
 * y^*}P_{y^*}^{-1}\f$. The mean and covariance of the new state
 * estimate may be calculated as:
 *
 * \f{eqnarray*}
 *   \hat{\mathbf{x}}(t_n) &=& \hat{\mathbf{x}} +
 *   \mathcal{K}(\hat{\mathbf{y}}^* - \mathbf{y}(t_n)) \\
 *   P_x(t_n) &=& P_x - \mathcal{K}P_{y^*}\mathcal{K}^T
 * \f}
 *
 * Note the use of an augmented state here -- we propagate
 * \f$(\mathbf{x}^T,\mathbf{v}^T)^T\f$ through \f$f\f$ and
 * \f$(\mathbf{x}^T,\mathbf{w}^T)^T\f$ through \f$g\f$, not
 * \f$\mathbf{x}\f$ alone. This is the reason that this class requires
 * information on the system noise \f$\mathbf{w}\f$ and measurement
 * noise \f$\mathbf{v}\f$, unlike other %filter classes.
 *
 * @section UnscentedKalmanFilter_references References
 *
 * @anchor Wan2000
 * Wan, E.A. & van der Merwe, R. The Unscented Kalman %Filter for
 * Nonlinear Estimation. <i>Proceedings of IEEE Symposium on Adaptive
 * Systems for Signal Processing Communications and Control
 * (AS-SPCC)</i>, <b>2000</b>.
 */
template<class T = unsigned int>
class UnscentedKalmanFilter : public Filter<T> {

  friend class UnscentedKalmanFilterTransitionAdaptor<T>;
  friend class UnscentedKalmanFilterMeasurementAdaptor<T>;

public:
  /**
   * Create new unscented Kalman %filter.
   *
   * @param model Model to estimate.
   * @param p_x0 \f$P\big(\mathbf{x}(0)\big)\f$; prior over the
   * initial state \f$\mathbf{x}(0)\f$.
   * @param p_w0 \f$P\big(\mathbf{w}(0)\big)\f$; prior over the
   * initial system noise \f$\mathbf{w}(0)\f$.
   * @param p_v0 \f$P\big(\mathbf{v}(0)\big)\f$; prior over the
   * initial measurement noise \f$\mathbf{v}(0)\f$.
   */
  UnscentedKalmanFilter(UnscentedKalmanFilterModel<T>* model,
      const indii::ml::aux::GaussianPdf& p_x0,
      const indii::ml::aux::GaussianPdf& p_w0,
      const indii::ml::aux::GaussianPdf& p_v0);

  /**
   * Destructor.
   */
  virtual ~UnscentedKalmanFilter();

  /**
   * Get the unscented transformation used for the transition
   * function. This allows its parameters to be adjusted.
   */
  UnscentedTransformation<T>* getTransitionTransformation();

  /**
   * Get the unscented transformation used for the measurement
   * function. This allows its parameters to be adjusted.
   */
  UnscentedTransformation<T>* getMeasurementTransformation();

  virtual void filter(T tnp1, const indii::ml::aux::vector& ytnp1);

  virtual indii::ml::aux::GaussianPdf measure();

protected:
  /**
   * Size of the state space.
   */
  const unsigned int N;

  /**
   * Size of the system noise space.
   */
  const unsigned int W;

  /**
   * Size of the measurement noise space.
   */
  const unsigned int V;

  /**
   * \f$P\big(\mathcal{X}(t_n)\, |
   * \,\mathbf{y}(t_1),\ldots,\mathbf{y}(t_n)\big)\f$; distribution
   * over the augmented state at the current time given the history of
   * measurements.
   */
  indii::ml::aux::GaussianPdf p_Xtn_ytn;

  /**
   * Model.
   */
  UnscentedKalmanFilterModel<T>* model;

  /**
   * Unscented transformation for transition function.
   */
  UnscentedTransformationModel<T>* transitionModel;

  /**
   * Unscented transformation for measurement function.
   */
  UnscentedTransformationModel<T>* measurementModel;

  /**
   * Unscented transformation for transition function.
   */
  UnscentedTransformation<T>* transitionTransform;

  /**
   * Unscented transformation for measurement function.
   */
  UnscentedTransformation<T>* measurementTransform;

};

    }
  }
}

#include "UnscentedKalmanFilterTransitionAdaptor.hpp"
#include "UnscentedKalmanFilterMeasurementAdaptor.hpp"
#include "UnscentedKalmanFilterModel.hpp"

#include "boost/numeric/bindings/traits/ublas_matrix.hpp"
#include "boost/numeric/bindings/traits/ublas_vector.hpp"
#include "boost/numeric/bindings/traits/ublas_symmetric.hpp"
#include "boost/numeric/bindings/lapack/lapack.hpp"

using namespace indii::ml::filter;

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

template <class T>
UnscentedKalmanFilter<T>::UnscentedKalmanFilter(
    UnscentedKalmanFilterModel<T>* model, const aux::GaussianPdf& p_x0,
    const aux::GaussianPdf& p_w0, const aux::GaussianPdf& p_v0) :
    Filter<T>(p_x0),
    N(p_x0.getDimensions()),
    W(p_w0.getDimensions()),
    V(p_v0.getDimensions()),
    p_Xtn_ytn(N+W+V),
    model(model) {
  /* unscented transformations */
  transitionModel = new UnscentedKalmanFilterTransitionAdaptor<T>(this);
  measurementModel = new UnscentedKalmanFilterMeasurementAdaptor<T>(this);
  transitionTransform = new UnscentedTransformation<T>(*transitionModel);
  measurementTransform = new UnscentedTransformation<T>(*measurementModel);

  /* ranges for subvectors and submatrices */
  ublas::range xRange(0,N);
  ublas::range wRange(N,N+W);
  ublas::range vRange(N+W,N+W+V);

  /* initialise augmented state */
  aux::vector mu(N+W+V);
  aux::symmetric_matrix sigma(N+W+V);

  noalias(project(mu, xRange)) = p_x0.getExpectation();
  noalias(project(mu, wRange)) = p_w0.getExpectation();
  noalias(project(mu, vRange)) = p_v0.getExpectation();

  noalias(project(sigma, xRange, xRange)) = p_x0.getCovariance();
  noalias(project(sigma, wRange, xRange)) = aux::zero_matrix(W, N);
  noalias(project(sigma, wRange, wRange)) = p_w0.getCovariance();
  noalias(project(sigma, vRange, xRange)) = aux::zero_matrix(V, N);
  noalias(project(sigma, vRange, wRange)) = aux::zero_matrix(V, W);
  noalias(project(sigma, vRange, vRange)) = p_v0.getCovariance();

  p_Xtn_ytn.setExpectation(mu);
  p_Xtn_ytn.setCovariance(sigma);
}

template <class T>
UnscentedKalmanFilter<T>::~UnscentedKalmanFilter() {
  delete transitionTransform;
  delete measurementTransform;
  delete transitionModel;
  delete measurementModel;
}

template <class T>
void UnscentedKalmanFilter<T>::filter(T tnp1, const aux::vector& ytnp1) {
  /* pre-condition */
  assert (tnp1 > Filter<T>::tn);
  assert (ytnp1.size() % V == 0);

  const T delta = tnp1 - Filter<T>::tn;
  Filter<T>::tn = tnp1;

  /* ranges for subvectors and submatrices */
  ublas::range xRange(0,N);
  ublas::range wRange(N,N+W);
  ublas::range vRange(N+W,N+W+V);

  /* unscented transformation of augmented state through transition
     function */
  aux::GaussianPdf p_xtnp1_ytn(transitionTransform->transform(p_Xtn_ytn,
      delta));

  /* prepare augmented state for measurement function */
  aux::vector mu(p_Xtn_ytn.getExpectation());
  aux::symmetric_matrix sigma(p_Xtn_ytn.getCovariance());

  noalias(project(mu, xRange)) = p_xtnp1_ytn.getExpectation();
  noalias(project(sigma, xRange, xRange)) = p_xtnp1_ytn.getCovariance();

  aux::GaussianPdf p_Xtnp1_ytn(mu, sigma);

  /* unscented transformation of augmented state through measurement
     function */
  aux::matrix P_xy(N+W+V,V); // cross-correlation matrix
  aux::GaussianPdf p_ytnp1_Xtnp1(measurementTransform->transform(p_Xtnp1_ytn,
      delta, &P_xy));

  /* update p_xtn_ytn based on measurement */
  const aux::vector &y_ytnp1_Xtnp1 = p_ytnp1_Xtnp1.getExpectation();
  const aux::symmetric_matrix &P_ytnp1_Xtnp1 = p_ytnp1_Xtnp1.getCovariance();
  const aux::vector &x_Xtnp1_ytn = p_Xtnp1_ytn.getExpectation();
  const aux::symmetric_matrix &P_Xtnp1_ytn = p_Xtnp1_ytn.getCovariance();

  /* 
   * Calculate Kalman gain directly without intermediate calculation
   * of inverse, improves performance marginally.
   *
   * Calculate Kalman gain. <tt>K = P_xy *
   * inv(P_ytnp1_Xtnp1)</tt>. Calculate as <tt>trans(K) =
   * inv(P_ytnp1_Xtnp1) * trans(P_xy), so that LAPACK gesv function
   * can be used, noting that <tt>trans(P_ytnp1_Xtnp1) ==
   * P_ytnp1_Xtnp1</tt> as <tt>P_ytnp1_Xtnp1</tt> is symmetric.
   *
   * Note that Kalman gain applies only to state variables, not noise
   * variables, so means and covariances can be projected down to only
   * state variables for the update.
   */
  aux::matrix X(P_ytnp1_Xtnp1);
  aux::matrix KT(trans(project(P_xy,xRange,ublas::range(0,V))));
  int err;
  err = lapack::gesv(X,KT);
  assert (err == 0);
  aux::matrix K(trans(KT));

  noalias(project(mu,xRange)) = project(x_Xtnp1_ytn,xRange) +
      prod(K, ytnp1 - y_ytnp1_Xtnp1);

  X.resize(P_ytnp1_Xtnp1.size1(), KT.size2(), false);
  noalias(X) = prod(P_ytnp1_Xtnp1, KT);
  noalias(project(sigma,xRange,xRange)) = project(P_Xtnp1_ytn,xRange,xRange)
      - prod(K, X);

  p_Xtn_ytn.setExpectation(mu);
  p_Xtn_ytn.setCovariance(sigma);
  Filter<T>::p_xtn_ytn.setExpectation(project(mu, xRange));
  Filter<T>::p_xtn_ytn.setCovariance(project(sigma, xRange, xRange));

  /* post-condition */
  assert (Filter<T>::tn == tnp1);
}

template <class T>
aux::GaussianPdf UnscentedKalmanFilter<T>::measure() {
  return aux::GaussianPdf(measurementTransform->transform(p_Xtn_ytn));
}

template <class T>
UnscentedTransformation<T>*
    UnscentedKalmanFilter<T>::getTransitionTransformation() {
  return transitionTransform;
}

template <class T>
UnscentedTransformation<T>*
    UnscentedKalmanFilter<T>::getMeasurementTransformation() {
  return measurementTransform;
}

#endif