odo3dPredictModel.hpp

/* $Id$ */

#ifndef SLAM_ODO_3D_PREDICT_MODEL_HPP
#define SLAM_ODO_3D_PREDICT_MODEL_HPP

#include "jmath/jblas.hpp"

#include "filter/predictModel.hpp"

namespace jafar {
  namespace slam {

    class OdoNoiseModel {

    public:
    
      double kvv;
      double kvw;
      double kwv;
      double kww;

      void set(double kvv_, double kvw_, double kwv_, double kww_) {
  kvv = kvv_;
  kvw = kvw_;
  kwv = kwv_;
  kww = kww_;
      }

  

      void computeCov(jblas::vec const& vw,
          jblas::sym_mat& vwCov) const
      {
  JFR_PRECOND(vw.size() == 2,
        "OdoNoiseModel::computeOdoCov: odo must be 2-dimension");
  JFR_PRECOND(vwCov.size1() == 2 && vwCov.size2() == 2,
        "OdoNoiseModel::computeOdoCov: odoCov must be a 2x2 matrix");

  vwCov.clear();
  vwCov(0,0) = pow(kvv,2)*fabs(vw(0)) + pow(kvw, 2)*fabs(vw(1));
  vwCov(1,1) = pow(kwv,2)*fabs(vw(0)) + pow(kww, 2)*fabs(vw(1));
      }

    };

  
    class Odo3dPredictModel :
      public jafar::filter::JacobianBlockCommandPredictModel {
      
    public:

      OdoNoiseModel odoNoiseModel;
      
      double kzs, kprs;

      double robotToRef_yaw;

      Odo3dPredictModel();
      ~Odo3dPredictModel() {}

      void setModel(double kvv_, double kvw_, double kwv_, double kww_, double kzv_, double kprs_);

      void computeUCov(jblas::vec const& u) {
  odoNoiseModel.computeCov(u, m_uCov);
      }

      void predict(jblas::vec_range& x_r, jblas::vec const& u);

    }; // class Odo3dPredictModel
  
  } // namespace slam
} // namespace jafar

#endif // SLAM_ODO_3D_PREDICT_MODEL_HPP