Kalman.hh

/* 
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS 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.

BIAS 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 BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/


#ifndef __Kalman_hh__
#define __Kalman_hh__

#include <Base/Debug/Debug.hh>
#include <Base/Math/Matrix.hh>
#include <Base/Math/Vector.hh>
#include <MathAlgo/SVD.hh>

namespace BIAS {

  /** @brief Classical Kalman filter. See http://www.cs.unc.edu/~welch/kalman/ for details.
      
      Initialize it by either
      - either calling Init
      - or calling the approbriate constructor
      with the initial system state, the system matrix and the measurement matrix.

      Use it by consecutively call
      - Predict()
      - Update()

      Extract the prediction (prior) by calling GetState() and 
      GetCovariance() after the Predict() step.
      
      Extract the estimated system state by calling GetState() and 
      GetCovariance() after the Update() step.
            @ingroup g_stateestimation      
      @author Felix Woelk 05/2005, untested so far */
  class BIASStateEstimator_EXPORT Kalman : public Debug
  {
  public:
    Kalman();
    
    Kalman(const Matrix<double>& SystemMatrix,
           const Matrix<double>& MeasurementMatrix,
           const Vector<double>& InitialSystemState,
           const Matrix<double>& InitialSystemCovariance);

    ~Kalman();

    void Init(const Matrix<double>& SystemMatrix,
              const Matrix<double>& MeasurementMatrix,
              const Vector<double>& InitialSystemState,
              const Matrix<double>& InitialSystemCovariance);

    /** Calculates the prediction using the system matrix A
        @author woelk 05/2005 */
    void Predict(const Matrix<double>& ProcessCovariance);

    /** Use this prediction function system in case of varying system matrix A.
        @author woelk 05/2005 */ 
    void Predict(const Matrix<double>& SystemMatrix,
                 const Matrix<double>& ProcessCovariance);

    /** Calculates the update step.
        @author woelk 05/2005 */
    void Update(const Vector<double>& Measurement, 
                const Matrix<double>& MeasurementCovariance);
    
    /** Returns the system state. It could be the prediction, if called after 
        the predict step or the posterior, if GetState is called after 
        the update step. 
        @author woelk 05/2005 */
    inline void GetState(Vector<double>& state) const
    { state = State_; }
    
    /** Returns the system state covariance. It could be the covariance of the 
        prediction, if called after the predict step or the covariance of the
        posterior, if GetState is called after the update step. 
        @author woelk 05/2005 */
    inline void GetCovariance(Matrix<double>& cov) const
    { cov = Covariance_; }

  protected:
    /// Contains either the prediction (prior) or Bayes optimal system estimate
    /// (posterior) depending what function (Predict()/Update()) wascalled last
    Vector<double> State_;
    /// The covariance matrix corresponding to State_
    Matrix<double> Covariance_;

    /// measurement matrix and transposed
    Matrix<double> H_, HT_;

    /// measurement matrix and transposed
    Matrix<double> A_, AT_;

    /// identity matrix
    Matrix<double> Identity_;

    /// dimensions of system space and measurement space
    int SystemStateDim_, MeasurementDim_;

    /// needed for matrix inversion
    SVD svd_;

  }; // class



} // namespace


#endif // __Kalman_hh__