kalmanFilter.hpp

/* $Id$ */

#ifndef FILTER_KALMAN_FILTER_HPP
#define FILTER_KALMAN_FILTER_HPP

#include <iostream>

#include "boost/date_time/posix_time/posix_time_types.hpp"

#include "kernel/jafarDebug.hpp"
#include "kernel/dataLog.hpp"

#include "jmath/jblas.hpp"
#include "jmath/gaussianVector.hpp"

#include "filter/filterException.hpp"
#include "filter/predictModel.hpp"
#include "filter/observeModel.hpp"
#include "filter/constraintModel.hpp"

namespace jafar {
  namespace filter {

    class BaseKalmanFilter : public jafar::kernel::DataLoggable {

    public:

      enum CovUpdateType {SIMPLE,   
                          STANDARD, 
                          JOSEPH    
      };

      enum ConsistencyCheckLevel {CONSISTENCY_NONE,     
          CONSISTENCY_WARNING,  
          CONSISTENCY_EXCEPTION 
      };

    protected:

      CovUpdateType covUpdateType;

      jblas::vec x;

      jblas::sym_mat P;

      boost::posix_time::time_duration p_curTime;

      jblas::mat K;

      jblas::mat KH_tmp;

      ConsistencyCheckLevel consistencyCheckLevel;
      double chi2Threshold;

      template<class Vec>
      void checkConsistency(Vec const& y, jblas::sym_mat const& Sinv) 
      {
  if (consistencyCheckLevel == CONSISTENCY_NONE)
    return;

  JFR_PRECOND(Sinv.size1() == y.size() && Sinv.size2() == y.size(),
        "BaseKalmanFilter::checkConsistency: invalid size of S");

  using namespace ublas;
  double ySiy = inner_prod(y, prod(Sinv, y) );
  checkConsistency(y, Sinv, ySiy);
      }

      void checkConsistency(double y, double Sinv) {
  if (consistencyCheckLevel == CONSISTENCY_NONE)
    return;
  
  double ySiy = y * Sinv * y;
  checkConsistency(y, Sinv, ySiy);
      }

      template<typename YType, typename SinvType> 
      void checkConsistency(YType y, SinvType Sinv, double ySiy) {
  if (ySiy > chi2Threshold) {
    switch(consistencyCheckLevel) {
    case CONSISTENCY_NONE:
      // shut up gcc warning
      break;
    case CONSISTENCY_WARNING:
      JFR_WARNING("ySiy=" << ySiy << " threshold=" << chi2Threshold
      << " y=" << y << " Sinv=" << Sinv  << ")");
      break;
    case CONSISTENCY_EXCEPTION:
      std::ostringstream s;
      s << "ySiy=" << ySiy << " threshold=" << chi2Threshold
        << " y=" << y << " Sinv=" << Sinv  << ")";
      throw InconsistentUpdateException(ySiy, chi2Threshold, s.str(), __FILE__, __LINE__);
      break;
    }
  }
      }

      // logging method
      void writeLogHeader(jafar::kernel::DataLogger& log) const;
      void writeLogData(jafar::kernel::DataLogger& log) const;

    public:

      BaseKalmanFilter(std::size_t size_);

      virtual ~BaseKalmanFilter();

      jblas::vec const& getX() const {return x;};
      jblas::vec& getX() {return x;};
      jblas::sym_mat const& getP() const {return P;};
      jblas::sym_mat& getP() {return P;};

      void clear();

      boost::posix_time::time_duration const& getCurrentTime() const {return p_curTime;}
      
      void setCovUpdateType(CovUpdateType covUpdateType_);
      CovUpdateType getCovUpdateType() const;

      void setupConsistencyCheck(ConsistencyCheckLevel level_, 
         double chi2Threshold_)
      {
  consistencyCheckLevel = level_; chi2Threshold = chi2Threshold_;
      }

      ConsistencyCheckLevel getConsistencyCheckLevel() const
      {
        return consistencyCheckLevel;
      }
      
      double getChi2Threshold() const
      {
        return chi2Threshold;
      }

      
      void init(const jblas::vec& state_, const jblas::sym_mat& cov_, 
    boost::posix_time::time_duration const& curTime);

      void init(const jblas::vec& state_, const jblas::sym_mat& cov_);

      void init(boost::posix_time::time_duration const& curTime);

      virtual std::size_t sizeState() const {return x.size();};

      //      void resize(std::size_t size_);

    }; // class BaseKalmanFilter

    class KalmanFilter : public BaseKalmanFilter {

    public :

      KalmanFilter(std::size_t size_);

      virtual ~KalmanFilter();

      void predict(const LinearPredictModel& m_);

#ifndef SWIG

      void predict(LinearCommandPredictModel& m_, jblas::vec const& u_);
#endif // SWIG

      void update(LinearObserveModel& m_, jblas::vec& z_);

    protected :

      void updateLinear(const jblas::mat_range& H_, 
                        const jblas::vec_range& z_, double R_);

    }; // class KalmanFilter

#ifndef SWIG
    
    class ExtendedKalmanFilter : public KalmanFilter {

    public :
  
      ExtendedKalmanFilter(std::size_t n_);

      virtual ~ExtendedKalmanFilter();

      void predict(JacobianPredictModel& m_);

      void predict(JacobianCommandPredictModel& m_, jblas::vec const& u_);
      
      void update(JacobianObserveModel& m_, const jblas::vec& z_);

    protected:

      void updateExtended(const jblas::mat_range& H_, 
                          double y_, double R_);

    }; // class ExtendedKalmanFilter

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

    class ExtendedKalmanFilterImplicit : public ExtendedKalmanFilter
    {
    public :
  
      ExtendedKalmanFilterImplicit(std::size_t n_);

      virtual ~ExtendedKalmanFilterImplicit();      


      void update(JacobianImplicitObserveModel& m_, 
      const jblas::vec& z_);

    }; // class ExtendedKalmanFilterImplicit


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

    class BlockExtendedKalmanFilter : public BaseKalmanFilter {

    public:

      jblas::vec_range* x_r;
      jblas::sym_mat_range* P_r;
  
      BlockExtendedKalmanFilter(std::size_t sizeMax_, std::size_t size_);

      virtual ~BlockExtendedKalmanFilter();

      inline std::size_t sizeStateMax() const {return x.size();};
      virtual inline std::size_t sizeState() const {return x_r->size();};

      void softResize(std::size_t size_);
  
      void predict(std::size_t index1_, std::size_t index2_,
       LinearPredictModel const& m_);

      void predict(std::size_t index1_, std::size_t index2_,
       JacobianBlockPredictModel& m_);

      void predict(std::size_t index1_, std::size_t index2_,
       JacobianBlockCommandPredictModel& m_, 
       jblas::vec const& u_);

      void update(LinearObserveModel& m_, std::size_t index, const jblas::vec& z_);

      void update(JacobianObserveModel& m_,  std::size_t index, jblas::vec const& z_);

      void update(JacobianObserveModel& m_,  
      std::size_t index_, 
      jblas::vec_range& x_,
      jblas::sym_mat_range& P_,
      jblas::vec const& z_);

//       void update(BlockObserveModel& m_,
//                   std::size_t index1_, std::size_t index2_,
//                   const jblas::vec& z_);

      void update(BlockObserveModel& m_,
                  std::size_t index1_, std::size_t index2_,
                  jblas::vec_range& x1_,
                  jblas::vec_range& x2_,
                  jblas::sym_mat_range& P1_,
                  jblas::sym_mat_range& P2_,
                  const jblas::vec& z_,
      jmath::GaussianVector& zPred);

      void applyConstraint(JacobianStrongConstraintModel& model,
         std::size_t index_);

      void applyConstraint(JacobianStrongConstraintModel& model,
         std::size_t index_,
         jblas::vec_range& x_,
         jblas::sym_mat_range& P_);

      double computeMahalanobisDistance(LinearObserveModel& m_,
                                        std::size_t index_,
                                        const jblas::vec& z_);

      double computeMahalanobisDistance(LinearObserveModel& m_,
                                        const jblas::vec_range& x_,
                                        const jblas::sym_mat_range& P_,
                                        const jblas::vec& z_);

      double computeMahalanobisDistance(BlockObserveModel& m_,
                                        std::size_t index1_, 
          std::size_t index2_,
                                        const jblas::vec_range& x1_,
                                        const jblas::vec_range& x2_,
                                        const jblas::sym_mat_range& P1_,
                                        const jblas::sym_mat_range& P2_,
                                        const jblas::vec& z_);
    protected:

      jblas::mat_range* K_r;
      jblas::mat_range* KH_tmp_r;

      void updateBlockExtended(std::size_t index_,
             const jblas::sym_mat_range& P_,
             const jblas::sym_mat_range& Pcol_,
             const jblas::mat_range& H_,
             double y_, double R_);

      void updateBlockExtended(std::size_t index_,
             const jblas::sym_mat_range& P_,
             const jblas::sym_mat_range& Pcol_,
             const jblas::mat& H_,
             jblas::vec const& y, 
             jblas::sym_mat const& R_);

      void updateBlockExtended(std::size_t index1_, std::size_t index2_,
             const jblas::sym_mat_range& P1_, 
             const jblas::sym_mat_range& P2_,
             const jblas::sym_mat_range& P12_, 
             const jblas::sym_mat_range& P21_,
             const jblas::sym_mat_range& Pcol1_, 
             const jblas::sym_mat_range& Pcol2_,
             const jblas::mat_range& H1_, 
             const jblas::mat_range& H2_,
             double y_, double R_);

      void updateBlockExtended(std::size_t index1_,
             std::size_t index2_,
             const jblas::sym_mat_range& P1_, 
             const jblas::sym_mat_range& P2_,
             const jblas::sym_mat_range& P12_, 
             const jblas::sym_mat_range& P21_,
             const jblas::sym_mat_range& Pcol1_, 
             const jblas::sym_mat_range& Pcol2_,
             const jblas::mat& H1_, 
             const jblas::mat& H2_,
             jblas::vec const& y_, 
             jblas::sym_mat const& R_);


      friend std::ostream& operator <<(std::ostream& s, 
               const BlockExtendedKalmanFilter& f_);

    }; // class BlockExtendedKalmanFilter

    std::ostream& operator <<(std::ostream& s, 
            const BlockExtendedKalmanFilter& f_);

#else // SWIG
    
    // we dont want to wrap those classes with swig, but we want swig
    // to be aware of the heritage relationship.

    class ExtendedKalmanFilter : public BaseKalmanFilter {};

    class ExtendedKalmanFilterImplicit : public ExtendedKalmanFilter {};

    class BlockExtendedKalmanFilter : public ExtendedKalmanFilter {};

#endif // SWIG

  } // namespace filter
} // namespace jafar

#endif // FILTER_KALMAN_FILTER_HPP