observeModel.hpp

/* $Id$ */

#ifndef FILTER_OBSERVE_MODEL_HPP
#define FILTER_OBSERVE_MODEL_HPP

#include "kernel/jafarException.hpp"

#include "jmath/jblas.hpp"

namespace jafar {
  namespace filter {

    class BaseObserveModel {
      
    public:

      BaseObserveModel(std::size_t sizeObs_, std::size_t sizeState_);
      BaseObserveModel(std::size_t sizeObs_, std::size_t sizeState_, std::size_t sizeInnovation_, std::size_t sizePrediction_);

      virtual ~BaseObserveModel();

      inline std::size_t sizeObs() const {return p_sizeObs;}
      inline std::size_t sizeState() const {return p_sizeState;}
      inline std::size_t sizeInnovation() const {return p_sizeInnovation;}
      inline std::size_t sizePrediction() const {return p_sizePrediction;}
      inline bool isCorrelated() const {return p_isCorrelated;}

      virtual jblas::sym_mat const& getR() const {return R;}

      void setCorrelatedR(jblas::sym_mat const& R_);

      void setUncorrelatedR(jblas::vec const& R_);

//       /** Compute observation noise covariance. A default
//        * implementation is provided, it does nothing (constant model).
//        */
//       virtual void computeR(jblas::vec const& z_);

    protected:
      std::size_t p_sizeObs;
      std::size_t p_sizeState;
      std::size_t p_sizeInnovation;
      std::size_t p_sizePrediction;

      bool p_isCorrelated;

      jblas::sym_mat R;

      mutable jblas::vec z;

    }; // class BaseObserveModel

    //======================================================

    class LinearObserveModel : public BaseObserveModel {
  
    public:

      jblas::mat H;

      LinearObserveModel(std::size_t sizeObs_, std::size_t sizeState_);
      LinearObserveModel(jblas::mat const& H_);

      ~LinearObserveModel();

      virtual jblas::vec const& predictObservation(const jblas::vec& x_) const;
      

      //       virtual jblas::vec const& computeInnovation(const jblas::vec& z_, 
      //             const jblas::vec& x_) const;


    }; // class LinearObserveModel

    //======================================================

    class JacobianObserveModel : public BaseObserveModel {

    public:

      jblas::mat Jobs;

      JacobianObserveModel(std::size_t sizeObs_, std::size_t sizeState_);

      virtual ~JacobianObserveModel();

      virtual jblas::vec const& predictObservation(jblas::vec const & x_) = 0;

      virtual jblas::vec const& computeInnovation(jblas::vec const& z_,
              jblas::vec const& zPred_) 
      {
  JFR_PRECOND(z_.size() == sizeObs(),
        "JacobianObserveModel::computeInnovation: size of z_ does not match");
  JFR_PRECOND(zPred_.size() == sizeObs(),
        "JacobianObserveModel::computeInnovation: size of zPred_ does not match");
  z = z_ - zPred_;
  return z;
      }

      virtual void predictObservationJac(const jblas::vec& x_) = 0;


    }; // class JacobianObserveModel



    //======================================================
    
    class JacobianImplicitObserveModel : public BaseObserveModel {

    public:

      jblas::mat Jobs_X;

      jblas::mat Jobs_Z;


      JacobianImplicitObserveModel(std::size_t sizeObs_, 
           std::size_t sizeState_,
           std::size_t functionCompenent);

      virtual ~JacobianImplicitObserveModel();

      // this funtion will compute the function of observation
      // at the h(x_t_t1,z_t) 
      virtual jblas::vec const& computeInnovation(const jblas::vec& x_,
                    const jblas::vec& z_) = 0;

      virtual void computeJacobian(const jblas::vec& x_,
           const jblas::vec& z_) = 0;

    }; // class JacobianImplicitObserveModel

    //======================================================




    class BlockObserveModel : public BaseObserveModel {

    protected:
    
      std::size_t _sizeState1;
      std::size_t _sizeState2;

    public:

      jblas::mat Jobs1;

      jblas::mat Jobs2;

      BlockObserveModel(std::size_t sizeObs_, 
      std::size_t sizeState1_, std::size_t sizeState2_);
      BlockObserveModel(std::size_t sizeObs_, 
      std::size_t sizeState1_, std::size_t sizeState2_, std::size_t sizeInnovation_, std::size_t sizePrediction_);

      virtual ~BlockObserveModel();

      inline std::size_t sizeState1() const {return _sizeState1;};
      inline std::size_t sizeState2() const {return _sizeState2;};


      virtual jblas::vec const& predictObservation(const jblas::vec_range& x1_,
               const jblas::vec_range& x2_) = 0;

      virtual jblas::vec const& computeInnovation(jblas::vec const& y_,
              jblas::vec const& zPred_) 
      {
        JFR_PRECOND(y_.size() == sizeObs(), "BlockObserveModel::computeInnovation: size of z_ does not match");
        JFR_PRECOND(zPred_.size() == sizeObs(), "BlockObserveModel::computeInnovation: size of zPred_ does not match");
        z = y_ - zPred_;
        return z;
      }

      virtual void predictObservationJac(const jblas::vec_range& x1_, 
                                         const jblas::vec_range& x2_) = 0;

    }; // class BlockObserveModel

  } // namespace filter
} // namespace jafar

#endif // FILTER_OBSERVE_MODEL_HPP