kalmanfilter.h

Go to the documentation of this file.

// $Id$
// Copyright (C) 2002 Klaas Gadeyne <first dot last at gmail dot com>
//                    Wim Meeussen  <wim dot meeussen at mech dot kuleuven dot ac dot be>
//                    Tinne De Laet <tinne dot delaet at mech dot kuleuven dot be>
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation; either version 2.1 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
//

#ifndef __KALMAN_FILTER__
#define __KALMAN_FILTER__

#include "filter.h"
#include "../pdf/gaussian.h"
#include "../model/analyticmeasurementmodel_gaussianuncertainty.h"
#include "../model/analyticsystemmodel_gaussianuncertainty.h"
# include <map>

namespace BFL
{


class KalmanFilter : public Filter<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>
{
public:

  KalmanFilter(Gaussian* prior);

  virtual ~KalmanFilter();

  // implement virtual function
  virtual Gaussian* PostGet();

  //  For realtime use, this function should be called before calling measUpdate
  /*  @param vector containing the dimension of the measurement models which are
      going to be used
  */
  void AllocateMeasModel( const vector<unsigned int>& meas_dimensions);

  //  For realtime use, this function should be called before calling measUpdate
  /*  @param dimension of the measurement models which is
      going to be used
  */
  void AllocateMeasModel( const unsigned int& meas_dimensions);

private:
  struct MeasUpdateVariables
  {
    Matrix _S_Matrix;
    Matrix _K;
    ColumnVector _innov;
    Matrix _postHT;
    MeasUpdateVariables() {};
    MeasUpdateVariables(unsigned int meas_dimension, unsigned int state_dimension):
      _S_Matrix(meas_dimension,meas_dimension)
    , _K(state_dimension,meas_dimension)
    , _innov(meas_dimension)
    , _postHT(state_dimension,meas_dimension)
{};
  }; //struct

protected:
  // variables to avoid allocation during update calls
  ColumnVector  _Mu_new;
  SymmetricMatrix _Sigma_new;
  Matrix _Sigma_temp;
  Matrix _Sigma_temp_par;
  Matrix _SMatrix;
  Matrix _K;
  std::map<unsigned int, MeasUpdateVariables> _mapMeasUpdateVariables;
  std::map<unsigned int, MeasUpdateVariables>::iterator _mapMeasUpdateVariables_it;


  friend class NonminimalKalmanFilter;

  void PostSigmaSet( const MatrixWrapper::SymmetricMatrix& s);

  void PostMuSet( const MatrixWrapper::ColumnVector& c);

  void CalculateSysUpdate(const MatrixWrapper::ColumnVector& J, const MatrixWrapper::Matrix& F, const MatrixWrapper::SymmetricMatrix& Q);

  void CalculateMeasUpdate(const MatrixWrapper::ColumnVector& z, const MatrixWrapper::ColumnVector& Z, const MatrixWrapper::Matrix& H, const MatrixWrapper::SymmetricMatrix& R);


  virtual void SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,
                         const MatrixWrapper::ColumnVector& u) = 0;


  virtual void MeasUpdate(MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel,
                          const MatrixWrapper::ColumnVector& z,
                          const MatrixWrapper::ColumnVector& s) = 0;

  virtual bool UpdateInternal(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,
                              const MatrixWrapper::ColumnVector& u,
                              MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel,
                              const MatrixWrapper::ColumnVector& z,
                              const MatrixWrapper::ColumnVector& s);

}; // class



} // End namespace BFL

#endif // __KALMAN_FILTER__