rauchtungstriebelsmoother.hpp

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

#ifndef INDII_ML_FILTER_RAUCHTUNGSTRIEBELSMOOTHER_HPP
#define INDII_ML_FILTER_RAUCHTUNGSTRIEBELSMOOTHER_HPP

#include "KalmanFilter.hpp"
#include "Smoother.hpp"
#include "RauchTungStriebelSmootherModel.hpp"

#include <stack>

namespace indii {
  namespace ml {
    namespace filter {

/**
 * Rauch-Tung-Striebel (RTS) 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.
 *
 * RauchTungStriebelSmoother is suitable for models with linear
 * transition and measurement functions, approximating state and noise
 * with indii::ml::aux::GaussianPdf distributions. The advantage of
 * the RauchTungStriebelSmoother compared to KalmanSmoother is that
 * the measurements are not required for the backwards pass.
 * 
 * @see indii::ml::filter for general usage guidelines.
 * @see LinearModel for more detail on linear filters.
 */
template <class T>
class RauchTungStriebelSmoother : public KalmanFilter<T>, public Smoother<T> {
public:
  /**
   * Create new RTS 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$.
   */
  RauchTungStriebelSmoother(RauchTungStriebelSmootherModel<T>* model,
      const indii::ml::aux::GaussianPdf& p_x0);

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

  /**
   * %Filter in the same manner as KalmanFilter.
   */
  virtual void filter(T tnp1, const indii::ml::aux::vector& ytnp1);

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

  /**
   * Rewind system to time of previous measurement and
   * smooth.
   *
   * @param tnm1 \f$t_{n-1}\f$; the time to which to rewind the
   * system. This must be less than the current time \f$t_n\f$.
   * @param p_xtnm1_ytnm1

   * \f$P\big(\mathbf{x}(t_{n-1})\, |
   * \,\mathbf{y}(t_1),\ldots,\mathbf{y}(t_{n-1})\big)\f$;
   * distribution over the state at time \f$t_{n-1}\f$ given past and
   * present measurements (i.e. the estimate from the forward
   * filtering pass at time \f$t_{n-1}\f$).
   */
  virtual void smooth(T tnm1,
      const indii::ml::aux::GaussianPdf& p_xtnm1_ytnm1);

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

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

};

    }
  }
}

using namespace indii::ml::filter;

namespace aux = indii::ml::aux;

template <class T>
RauchTungStriebelSmoother<T>::RauchTungStriebelSmoother(
    RauchTungStriebelSmootherModel<T>* model, const aux::GaussianPdf& p_x0)
    : KalmanFilter<T>(model, p_x0), Smoother<T>(p_x0), model(model) {
  //
}

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

template <class T>
void RauchTungStriebelSmoother<T>::filter(T tnp1, const aux::vector& ytnp1) {
  KalmanFilter<T>::filter(tnp1, ytnp1);
  Smoother<T>::p_xtn_ytT = Filter<T>::p_xtn_ytn;
}

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

  /* rewind time */
  T delta = Filter<T>::tn - tnm1;
  Filter<T>::tn = tnm1;

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

  /* calculate smoothed state for this time */
  aux::GaussianPdf p_xtnp1_ytn(model->p_xtnp1_ytn(Filter<T>::p_xtn_ytn,delta));
  Smoother<T>::p_xtn_ytT = model->p_xtn_ytT(Smoother<T>::p_xtn_ytT,
      p_xtnp1_ytn, Filter<T>::p_xtn_ytn, delta);

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

template <class T>
aux::GaussianPdf RauchTungStriebelSmoother<T>::measure() {
  return model->p_y_x(Filter<T>::p_xtn_ytn);
}

template <class T>
aux::GaussianPdf RauchTungStriebelSmoother<T>::smoothedMeasure() {
  return model->p_y_x(Smoother<T>::p_xtn_ytT);
}

#endif