IteratedExtendedKalman.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 __ITEREXTKALMAN_HH__
#define __ITEREXTKALMAN_HH__

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

namespace BIAS {

  /** 
      @class IteratedExtendedKalman

      @ingroup g_stateestimation

      @brief Iterated Extended Kalman-filter (IEKF). Implemented according to:
      www.cs.unc.edu/~tracker/media/pdf/SIGGRAPH2001_CoursePack_08.pdf [1] with
      additional iteration option. Implements an Iterated Extended Kalman 
      Filter, estimating the state of a system driven by a given possibly non-
      linear statetransition-funcition. Derive from this class an implement the
      virtual prediction functions (state transition, measurement prediction).
      Jacobians for model functions and according noise models have to be given.

      @author apetersen 05.08.10
  */

  class BIASStateEstimator_EXPORT IteratedExtendedKalman : public Debug
  {
  public:
    /** Standard constructor. Initiates object using maxIters=20, iterEps=1e-4
        and bUseStable=false.
     */
    IteratedExtendedKalman();
    
    /** Constructor using given parameters.
        @params maxIters maximum iterations for update
        @params iterEps break update iteration if update < iterEps
        @params bUseStable use stable (slower) update equation
     */
    IteratedExtendedKalman(unsigned int maxIters,
                           double iterEps,
                           bool bUseStable=false);

    /** Virtual standard destructor
     */
    virtual ~IteratedExtendedKalman() {};

    /** Returns the predicted (a priori) system state.
    */
    inline void GetPriorState(BIAS::Vector<double>& state) const
    { state = statePrior_; }
    
    /** Returns the last predicted (a priori) system state covariance.
    */
    inline void GetPriorCovariance(BIAS::Matrix<double>& cov) const
    { cov = covariancePrior_; }

    /** Returns the last corrected (a posteriori) system state.
    */
    inline void GetPosterioriState(BIAS::Vector<double>& state) const
    { state = statePost_; }
    
    /** Returns the updated (a posteriori) system state covariance.
    */
    inline void GetPosterioriCovariance(BIAS::Matrix<double>& cov) const
    { cov = covariancePost_; }

    /** Set a modified a state. Use this if an external transformation has been
        applied to the state (e.g. coordinate transformations) or the state 
        dimension has changed. Modify state covariance accordingly.
    */
    inline void SetState(BIAS::Vector<double>& state, BIAS::Matrix<double>& cov)
    {
#ifdef BIAS_DEBUG
      BIASASSERT(state.size() == cov.num_cols());
      BIASASSERT(state.size() == cov.num_rows());
#endif //BIAS_DEBUG

      state_ = state;
      covariance_ = cov;
      systemStateDim_ = state_.size();
    }

    /** Set the process noise covariance -- Q in [1]
        --- May be updated for every timestep! ---
     */
    inline void SetProcessCov(const BIAS::Matrix<double>& processNoiseCov)
    {
#ifdef BIAS_DEBUG
      BIASASSERT(processNoiseCov.num_cols() == processNoiseCov.num_rows());
#endif //BIAS_DEBUG

      Q_ = processNoiseCov;
      processNoiseDim_ = Q_.num_rows();
    }

    /** Initialize the kalman filter before using update/predict.
        @params initialState initial state
        @params initialCov covariance for initial state
        @params processNoiseCov process (prediction) noise covariance
     */
    void Init(const BIAS::Vector<double>& initialState,
              const BIAS::Matrix<double>& initialCov,
              const BIAS::Matrix<double>& processNoiseCov);
    
    /** Implements the statetransition-function (f in [1]). Predict the 'state'
        using the transition function and 'control' vector to get 'prediction'!
        Has to be implemented in child class!
        @params state a posterior state
        @params control vector containing parameters for prediction
        @params prediction predicte a priori state
     */
    virtual void StateTransition(const BIAS::Vector<double>& state,
                                 const BIAS::Vector<double>& control,
                                 BIAS::Vector<double>& prediction)
    {
      BIASERR("StateTransition not implemented in base class!");
      BIASABORT;
    }

    /** Returns the jacobian matrices (by state and process noise) for state 
        transition function. The matrices will be atomatically initialized 
        according to the system state/process noise bevore passed to function
        call.
        Has to be implemented in child class!
        @params state state used for prediction
        @params control control vector used for prediction
        @params transJacobian jacobian matrix of transition funciton (by state)
        @params transErrorJacobian jacobian matrix of TF (by process noise)
    */
    virtual void GetTransitionJacobian(const BIAS::Vector<double>& state,
                                       const BIAS::Vector<double>& control,
                                       BIAS::Matrix<double>& transJacobian,
                                       BIAS::Matrix<double>& transErrorJacobian)
    {
      BIASERR("GetTransitionJacobian not implemented in base class!");
      BIASABORT;
    }

    /** Implements a single preditcion step (with respect to the control 
        parameters -- u[k] in [1]). It can be called more than once between two
        updates (iterativ prediction).
        @params control control vector (u[k]) to be used for prediction
    */
    void Predict(const BIAS::Vector<double>& control);

    /** Implements the measurement prediction function (h in [1])! Since more 
        than one measurement is allowed, an index for determining the respective
        function can be given.
        Has to be implemented in child class!
        @params state the state to predict the measurement from
        @params measurePred the predicted measurement
        @params index the index for the measurement funtion to be applied
    */
    virtual void MeasurementPrediction(const BIAS::Vector<double>& state,
                                       BIAS::Vector<double>& measurePred,
                                       const unsigned int index=1)
    {
      BIASERR("StateTransition not implemented in base class!");
      BIASABORT;
    }

    /** Returns the jacobian matrix of the measurement-function with respect to
        the state (H in [1]) as well as the jacobian with respect to the noise
        model (V in [1]). The matrices will be atomatically initialized 
        according to the measurement/measurement noise bevore passed to function
        call.
        Has to be implemented in child class.
        @params state the state used for measurement prediction
        @params measJacobian jacobian matrix of measurement funciton (by state)
        @params measErrorJacobian jacobian matrix of MF (by process noise)
    */
    virtual void GetMeasureJacobian(const BIAS::Vector<double> &state,
                                    BIAS::Matrix<double> &measJacobian,
                                    BIAS::Matrix<double> &measErrorJacobian,
                                    const unsigned int index=1)
    {
      BIASERR("GetMeasureJacobian not implemented in base class!");
      BIASABORT;
    }

    /** Implememts the iterative update step using measurement and its 
        covariance. Can be call more than once between two predicts. Index 
        indicates which measurement funciton shall be used.
        @params measurement measurement vector
        @params measureNoise covariance for measurement
        @params index indicator for used meansurement function
    */
    void Update(const BIAS::Vector<double> &measurement,
                const BIAS::Matrix<double> &measureNoise,
                const unsigned int index);
    
  protected:

    /// flag for using numerically more stable computations for update
    /// (higher computation effort)
    const bool bUseStable_;

    /// number of maximal iterations for iterated update
    unsigned int maxIters_;

    /// break update iteration if update improvement is smaller than iterEps
    double iterEps_;

    /// contains the actual system estimate
    BIAS::Vector<double> state_;
    
    /// contains the last prediction (a priori/initial) system estimate
    BIAS::Vector<double> statePrior_;
    
    /// contains the corrected (a posteriori) system estimate
    BIAS::Vector<double> statePost_;

    /// identity matrix frequently used in Update
    BIAS::Matrix<double> Identity_;
    
    /// the covariance matrix corresponding to state_ 
    BIAS::Matrix<double> covariance_;

    /// the covariance matrix corresponding to statePrior_ 
    BIAS::Matrix<double> covariancePrior_;

    /// the covariance matrix corresponding to statePost_ 
    BIAS::Matrix<double> covariancePost_;

    /// measurement jacobian for measurement model and its transpose
    BIAS::Matrix<double> H_, HT_;

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

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

    /// state-trans. error jacobian and transposed
    BIAS::Matrix<double> W_, WT_;

    /// measurement error jacobian for measurement model and its transpose
    BIAS::Matrix<double> V_, VT_;

    /// dimension of state space
    unsigned int systemStateDim_;

    /// dimension of process noise
    unsigned int processNoiseDim_;

    /// needed for matrix inversion
    BIAS::SVD svd_;

  };

} // namespace BIAS


#endif // __ITEREXTKALMAN_HH__