ExtendedKalman.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 __EXTKALMAN_HH__
#define __EXTKALMAN_HH__

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

namespace BIAS {

  /** 
      @class ExtendedKalman
      @ingroup g_stateestimation
      @brief Classical ExtendedKalman-filter (EKF). Implemented according to:
                         www.cs.unc.edu/~tracker/media/pdf/SIGGRAPH2001_CoursePack_08.pdf [1]
                         Implements an Extended Kalman Filter (non-linear),estimating the state of a 
             system governed by a given non-linear statetransition-funcition.
             Derive from this class an implement the virtual statetrans.- and 
             measurement-function. After setting the differentiation matrices
             you will get an EKF for your specific problem.
      @author apetersen 03.04.07
  */

  class BIASStateEstimator_EXPORT ExtendedKalman : public Debug
  {
  public:
    ExtendedKalman();

    virtual ~ExtendedKalman(){};

    /** Returns the predicted system state. This is the prediction, it is
        to be called after the predict step!
    */
    inline void GetState(Vector<double>& state) const
    { state = priorState_; }
    
    /** Returns the corrected system state, which is the update-corrected state.
        This is to be called after the update step!
    */
    inline void GetStatePosteriori(Vector<double>& state) const
    { state = posterioState_; }
    
    /** 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.
    */
    inline void GetCovariance(Matrix<double>& cov) const
    { cov = covariance_; }

    /** Implement this to correlate the actual state with the 'predicted'
        measurement (h in [1])! 
        It should 'extract' the measurement from the state!
    */
    virtual void MeasurementFunction(const Vector<double>& state,
                                     Vector<double>& measurePred) = 0;

    /** Does preditcion (with respect to the control parameters -- u in [1]) 
        for a single timestep! (use in alternation with Update(), Update first)
    */
    int Predict(const Vector<double>& control);

    /** Use this function to set the initial state and system-covariance (x(0) 
        and P(0) in [1])!
     */
    inline void SetInitial(const Vector<double>& initialState,
                           const Matrix<double>& initialCov)
    { 
      priorState_ = initialState; 
      covariance_ = initialCov; 
      systemStateDim_ = initialState.size(); 
    }

    /** Same as above for measurement function! The DerivedMeasurement matrix 
        (H in [1]) contains the partial derivates of the measurement-function 
        (h in [1]) with respect to each state komponent (dhi/dxj(i,j)). The 
        matrix DerivedMeasurementError (V in [1]) contains the partial 
        derivatives of h with respect to each measurement-noise-komponent 
        (dhi/dvj(i,j)).

        These matrices may be changed for every timestep! 
        (according to the 'real' derivatives h)
    */
    inline void SetMeasureDerive(const Matrix<double>& derivedMeasurement,
                                 const Matrix<double>& derivedMeasurementError)
    {
      H_ = derivedMeasurement; HT_ = derivedMeasurement.Transpose();
      V_ = derivedMeasurementError; VT_ = derivedMeasurementError.Transpose();
    }

    /** Set measurement noise covariance -- R in [1]
        (may be updated for every timestep)!
        Measurement noise is assumed to be white and normaly distributed.
     */
    inline void SetMeasureCov(const Matrix<double>& measureNoiseCov)
    {R_ = measureNoiseCov;}

    /** Set the process noise covariance -- Q in [1]
        (may be updated for every timestep)!
        Process noise is assumed to be white and normaly distributed.
     */
    inline void SetProcessCov(const Matrix<double>& processNoiseCov)
    {Q_ = processNoiseCov;}

    /** Set the differentiation-matrices for the EKF (see [1] for details). 
        The DerivedStateTrans matrix (A in [1]) contains the partial deriva-
        tives of the statetrans.-function (f in [1]) with respect to each 
        state komponent (dfi/dxj(i,j)). The matrix DerivedStateTransError 
        (W in [1]) contains the partial derivatives of f with respect to each 
        process-noise-komponent (dfi/dwj(i,j)). 

        These matrices may be changed for every timestep!
        (according to the 'real' derivatives of f)
    */
    inline void SetStateDerive(const Matrix<double>& derivedStateTrans,
                               const Matrix<double>& derivedStateTransError)
    {
      A_ = derivedStateTrans; AT_ = derivedStateTrans.Transpose();
      W_ = derivedStateTransError; WT_ = derivedStateTransError.Transpose();
    }

    /** Implement the statetransition-function (f in [1]) here.
        It should compute the state x(k) for timestep k from the state x(k-1)
        for timestep k-1 and the control-params u(k)!
     */
    virtual void StateTransition(const Vector<double>& oldState,
                                 const Vector<double>& control,
                                 Vector<double>& newState) = 0;

    /** Calculates the update step. 
        (use in alternation with Predict(), Update first)
    */
    int Update(const Vector<double>& measurement);
    
  protected:
    /// Contains the prediction (a priori/initial) system estimate
    Vector<double> priorState_;
    
    /// Contains the corrected (a posteriori) system estimate
    Vector<double> posterioState_;
    
    /// The covariance matrix corresponding to State_ 
    Matrix<double> covariance_;

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

    /// measurement noise cov.
    Matrix<double> R_;

    /// derived state-trans. matrix and transposed
    Matrix<double> A_, AT_;

    /// system noise cov.
    Matrix<double> Q_;

    /// derived state-trans. error matrix and transposed
    Matrix<double> W_, WT_;

    /// derived measurement error matrix and transposed
    Matrix<double> V_, VT_;

    /// identity matrix
    Matrix<double> Identity_;

    /// and finally the Kalman gain
    Matrix<double> K_;
    
    /// maybe needed for iterated update
    Vector<double> lastKalmanUpdate_;

    /// dimensions of system space and measurement space
    int systemStateDim_, measurementDim_;

    /// needed for matrix inversion
    SVD svd_;

  };



} // namespace BIAS


#endif // __EXTKALMAN_HH__