kalmansmoother.hpp

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

#ifndef INDII_ML_FILTER_KALMANSMOOTHER_HPP
#define INDII_ML_FILTER_KALMANSMOOTHER_HPP

#include "KalmanFilter.hpp"
#include "TwoFilterSmoother.hpp"
#include "KalmanSmootherModel.hpp"

namespace indii {
  namespace ml {
    namespace filter {

/**
 * 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 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 KalmanFilter. 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.
 *
 * It is suitable for models with linear transition and measurement
 * functions, approximating state and noise with
 * indii::ml::aux::GaussianPdf distributions.
 *
 * @warning This implementation is buggy and requires further work!
 * Consider RauchTungStriebelSmoother instead.
 *
 * @todo Buggy, see test results.
 * 
 * @see indii::ml::filter for general usage guidelines.
 * @see LinearModel for more detail on linear filters.
 */
template <class T>
class KalmanSmoother : public KalmanFilter<T>, public TwoFilterSmoother<T> {
public:
  /**
   * Create new 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$.
   */
  KalmanSmoother(KalmanSmootherModel<T>* model,
      const indii::ml::aux::GaussianPdf& p_x0);

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

  /**
   * %Filter in the same manner as KalmanFilter.
   */
  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();

private:
  /**
   * Model to estimate.
   */
  KalmanSmootherModel<T>* model;

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

    }
  }
}

#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>
KalmanSmoother<T>::KalmanSmoother(KalmanSmootherModel<T>* model,
    const aux::GaussianPdf& p_x0) :
    KalmanFilter<T>(model, p_x0),
    TwoFilterSmoother<T>(p_x0),
    model(model),
    N(p_x0.getDimensions()) {
  //
}

template <class T>
KalmanSmoother<T>::~KalmanSmoother() {
  //
}

template <class T>
void KalmanSmoother<T>::filter(T tnp1, const aux::vector& ytnp1) {
  KalmanFilter<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;
}

template <class T>
void KalmanSmoother<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;

  /* backward filter */
  aux::GaussianPdf p_xtn_ytnp1_b(model->p_xtnm1_ytn(
      TwoFilterSmoother<T>::p_xtn_ytn_b, delta));
  TwoFilterSmoother<T>::p_xtn_ytn_b = model->p_xtnm1_ytnm1(p_xtn_ytnp1_b,
      ytnm1, delta);

  /* 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);
  aux::matrix X(N,N);

  /* 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) = 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 KalmanSmoother<T>::smoothedMeasure() {
  return model->p_y_x(Smoother<T>::p_xtn_ytT);
}

template <class T>
aux::GaussianPdf KalmanSmoother<T>::backwardMeasure() {
  return model->p_y_x(TwoFilterSmoother<T>::p_xtn_ytn_b);
}

#endif