unscentedkalmansmoother.hpp

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

#ifndef INDII_ML_FILTER_UNSCENTEDKALMANSMOOTHER_HPP
#define INDII_ML_FILTER_UNSCENTEDKALMANSMOOTHER_HPP

#include "UnscentedKalmanFilter.hpp"
#include "TwoFilterSmoother.hpp"

namespace indii {
  namespace ml {
    namespace filter {

      template<class T> class UnscentedKalmanSmootherModel;
      template<class T> class UnscentedKalmanSmootherBackwardTransitionAdaptor;

/**
 * Unscented Kalman two-filter smoother.
 *
 * @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.
 *
 * The Unscented Kalman two-filter smoother performs both a forward
 * and backwards filtering pass, fusing the estimates from the two
 * passes into the smoothed estimate. The forwards filtering pass is
 * identical to that of UnscentedKalmanFilter. The backwards filtering
 * pass requires the inverse of the state transition model, and works
 * in an analogous fashion to the forwards pass, but with time
 * reversed.
 * 
 * @see indii::ml::filter for general usage guidelines.
 */
template <class T>
class UnscentedKalmanSmoother : public UnscentedKalmanFilter<T>,
    public TwoFilterSmoother<T> {

  friend class UnscentedKalmanSmootherBackwardTransitionAdaptor<T>;

public:
  /**
   * Create new Unscented Kalman two-filter smoother.
   *
   * @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$.
   */
  UnscentedKalmanSmoother(UnscentedKalmanSmootherModel<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 ~UnscentedKalmanSmoother();

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

  virtual void smooth(T tnm1, const indii::ml::aux::vector& ytnm1,
      const indii::ml::aux::GaussianPdf& p_xtnm1_ytnm1);

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

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

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

protected:
  /**
   * \f$P\big(\mathcal{X}(t_n)\, |
   * \,\mathbf{y}(t_n),\ldots,\mathbf{y}(t_T)\big)\f$; distribution
   * over the augmented state at the current time given present and
   * future measurements.
   */
  aux::GaussianPdf p_Xtn_ytn_b;

  /**
   * Model to estimate.
   */
  UnscentedKalmanSmootherModel<T>* model;

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

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

};

    }
  }
}

#include "UnscentedKalmanSmootherBackwardTransitionAdaptor.hpp"

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

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

using namespace indii::ml::filter;

template <class T>
UnscentedKalmanSmoother<T>::UnscentedKalmanSmoother(
    UnscentedKalmanSmootherModel<T>* model, const aux::GaussianPdf& p_x0,
    const aux::GaussianPdf& p_w0, const aux::GaussianPdf& p_v0) :
    UnscentedKalmanFilter<T>(model, p_x0, p_w0, p_v0),
    TwoFilterSmoother<T>(p_x0), p_Xtn_ytn_b(p_x0.getDimensions()
        + p_w0.getDimensions() + p_v0.getDimensions()), model(model) {
  /* unscented transformations */
  backwardTransitionModel =
      new UnscentedKalmanSmootherBackwardTransitionAdaptor<T>(this);
  backwardTransitionTransform = new UnscentedTransformation<T>(
      *backwardTransitionModel);
}

template <class T>
UnscentedKalmanSmoother<T>::~UnscentedKalmanSmoother() {
  delete backwardTransitionTransform;
  delete backwardTransitionModel;
}

template <class T>
void UnscentedKalmanSmoother<T>::filter(T tnp1, const aux::vector& ytnp1) {
  UnscentedKalmanFilter<T>::filter(tnp1, ytnp1);

  /* initialisations for backward pass and smoothing */
  Smoother<T>::p_xtn_ytT = Filter<T>::p_xtn_ytn;
  TwoFilterSmoother<T>::p_xtn_ytn_b = Filter<T>::p_xtn_ytn;
  p_Xtn_ytn_b = UnscentedKalmanFilter<T>::p_Xtn_ytn;
}

template <class T>
void UnscentedKalmanSmoother<T>::smooth(T tnm1, const aux::vector& ytnm1,
      const aux::GaussianPdf& p_xtnm1_ytnm1) {
  /* pre-condition */
  assert (tnm1 < Filter<T>::tn);

  T delta = Filter<T>::tn - tnm1;
  Filter<T>::tn = tnm1;

  /* restore filtered state */
  Filter<T>::p_xtn_ytn = p_xtnm1_ytnm1;

  /* for convenience */
  const unsigned int N = UnscentedKalmanFilter<T>::N;
  const unsigned int W = UnscentedKalmanFilter<T>::W;
  const unsigned int V = UnscentedKalmanFilter<T>::V;
  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 backward
     transition function */
  aux::GaussianPdf p_xtn_ytnp1_b(backwardTransitionTransform->transform(
      p_Xtn_ytn_b, delta));

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

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

  aux::GaussianPdf p_Xtnm1_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_ytnm1_Xtnm1(
      UnscentedKalmanFilter<T>::measurementTransform->transform(p_Xtnm1_ytn,
      delta, &P_xy));

  /* update p_xtn_ytn_b based on measurement */
  const aux::vector &y_ytnm1_Xtnm1 = p_ytnm1_Xtnm1.getExpectation();
  const aux::symmetric_matrix &P_ytnm1_Xtnm1 = p_ytnm1_Xtnm1.getCovariance();
  const aux::vector &x_Xtnm1_ytn = p_Xtnm1_ytn.getExpectation();
  const aux::symmetric_matrix &P_Xtnm1_ytn = p_Xtnm1_ytn.getCovariance();
  aux::matrix PI_ytnm1_Xtnm1(V,V); // inverse of P_ytnm1_Xtnm1
  aux::matrix X(P_ytnm1_Xtnm1);
  aux::inv(X, PI_ytnm1_Xtnm1);
  aux::matrix K(prod(P_xy, PI_ytnm1_Xtnm1)); // Kalman gain

  /* only state variables should be updated, so zero out entries for noise
   * variables in the Kalman gain matrix */
  noalias(project(K,wRange,ublas::range(0,V))) = aux::zero_matrix(W,V);
  noalias(project(K,vRange,ublas::range(0,V))) = aux::zero_matrix(V,V);

  noalias(mu) = x_Xtnm1_ytn + prod(K, ytnm1 - y_ytnm1_Xtnm1);
  X.resize(V, N+W+V, false);
  noalias(X) = prod(P_ytnm1_Xtnm1, trans(K));
  noalias(sigma) = P_Xtnm1_ytn - prod(K, X);

  p_Xtn_ytn_b.setExpectation(mu);
  p_Xtn_ytn_b.setCovariance(sigma);
  TwoFilterSmoother<T>::p_xtn_ytn_b.setExpectation(project(mu, xRange));
  TwoFilterSmoother<T>::p_xtn_ytn_b.setCovariance(project(sigma, xRange,
      xRange));

  /* fuse p_xtn_ytn and p_xtn_ytnp1_b */
  const aux::symmetric_matrix& P_xtn_ytn= Filter<T>::p_xtn_ytn.getCovariance();
  const aux::symmetric_matrix& P_xtn_ytnp1_b = p_xtn_ytnp1_b.getCovariance();
  aux::matrix PI_xtn_ytn(N,N);
  aux::matrix PI_xtn_ytnp1_b(N,N);
  X.resize(N,N,false);

  /* calculate inverses */
  noalias(X) = P_xtn_ytn;
  aux::inv(X, PI_xtn_ytn);

  noalias(X) = P_xtn_ytnp1_b;
  aux::inv(X, PI_xtn_ytnp1_b);

  /* calculate fused covariance */
  aux::symmetric_matrix P_xtn_ytT(N);
  aux::matrix Y(N,N);
  noalias(Y) = PI_xtn_ytn + PI_xtn_ytnp1_b;

  /**
   * @todo Subtract sigma_{t}^{-1}, see Jordan 15.7.2 end.
   */
  aux::inv(Y,X);
  noalias(P_xtn_ytT) = ublas::symmetric_adaptor<aux::matrix, ublas::lower>(X);

  /* calculate fused mean */
  const aux::vector& x_xtn_ytn = Filter<T>::p_xtn_ytn.getExpectation();
  const aux::vector& x_xtn_ytnp1_b = p_xtn_ytnp1_b.getExpectation();
  aux::vector x_xtn_ytT(N);

  noalias(x_xtn_ytT) = prod(PI_xtn_ytn, x_xtn_ytn);
  noalias(x_xtn_ytT) += prod(PI_xtn_ytnp1_b, x_xtn_ytnp1_b);
  x_xtn_ytT = prod(P_xtn_ytT, x_xtn_ytT);

  /* update smoothed state */
  Smoother<T>::p_xtn_ytT.setExpectation(x_xtn_ytT);
  Smoother<T>::p_xtn_ytT.setCovariance(P_xtn_ytT);

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

template <class T>
aux::GaussianPdf UnscentedKalmanSmoother<T>::backwardMeasure() {
  return aux::GaussianPdf(
      UnscentedKalmanFilter<T>::measurementTransform->transform(p_Xtn_ytn_b));
}

template <class T>
aux::GaussianPdf UnscentedKalmanSmoother<T>::smoothedMeasure() {
  /* for convenience */
  const unsigned int N = UnscentedKalmanFilter<T>::N;
  const unsigned int W = UnscentedKalmanFilter<T>::W;
  const unsigned int V = UnscentedKalmanFilter<T>::V;
  ublas::range xRange(0,N);
  ublas::range wRange(N,N+W);
  ublas::range vRange(N+W,N+W+V);

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

  noalias(project(mu, xRange)) = Smoother<T>::p_xtn_ytT.getExpectation();
  noalias(project(sigma, xRange, xRange)) =
      Smoother<T>::p_xtn_ytT.getCovariance();

  aux::GaussianPdf p_Xtn_ytT(mu, sigma);

  return UnscentedKalmanFilter<T>::measurementTransform->transform(p_Xtn_ytT);
}

template <class T>
UnscentedTransformation<T>*
    UnscentedKalmanSmoother<T>::getBackwardTransitionTransformation() {
  return backwardTransitionTransform;
}

#endif