robotConstantMotionModel.hpp

#ifndef ROBOTCONSTANTMOTIONMODEL_HPP_
#define ROBOTCONSTANTMOTIONMODEL_HPP_

#include "rtslam/robotAbstract.hpp"

#define PERT_DETERMINISTIC 0


namespace jafar {
  namespace rtslam {


    template<unsigned LIN_ORDER, unsigned ANG_ORDER>
    class RobotConstantMotionModel: public RobotAbstract {
      private:
        ublas::range iaPos, iaOriQuat, iaAbsVel, iaAngVel, iaAbsAcc, iaAngAcc, iaAbsJerk, iaAngJerk;
      public:
        RobotConstantMotionModel(const map_ptr_t & _mapPtr);

        ~RobotConstantMotionModel(void) {}

        virtual std::string typeName() const {
          return "Constant-motion-model of order " + std::string(1,'0'+LIN_ORDER) + "/" + std::string(1,'0'+ANG_ORDER);
        }


        void move_func(const vec & _x, const vec & _u, const vec & _n, const double _dt, vec & _xnew, mat & _XNEW_x,
            mat & _XNEW_pert, unsigned tempSet = 0) const;

        void computePertJacobian();

        static size_t size() {
          return 7+LIN_ORDER*3+ANG_ORDER*3;
        }
        static size_t size_control() {
          return 0;
        }
        static size_t size_perturbation() {
          return 6;
        }

        virtual size_t mySize() {return size();}
        virtual size_t mySize_control() {return size_control();}
        virtual size_t mySize_perturbation() {return size_perturbation();}

        void setVelocityStd(double velLinStd, double velAngStd)
        {
          double factor = 1.;
          unsigned start = pose.size();
          for(size_t j = 0; j < LIN_ORDER; ++j)
          {
            if (j < LIN_ORDER)
            {
              for (size_t i = start; i < start + 3; ++i)
                state.P(i,i) = velLinStd*velLinStd*factor;
              start += 3;
            }
            if (j < ANG_ORDER)
            {
              for (size_t i = start; i < start + 3; ++i)
                state.P(i,i) = velAngStd*velAngStd*factor;
              start += 3;
            }
            factor *= 100;
          }
        }

        virtual void writeLogHeader(kernel::DataLogger& log) const;
        virtual void writeLogData(kernel::DataLogger& log) const;
        
      protected:
        template<class Vx, class Vp, class Vq, class Vv, class Vw,
                class Vva, class Vwa, class Vvj, class Vwj>
        inline void splitState(const Vx x, Vp & p, Vq & q, Vv & v, Vw & w,
          Vva & va, Vwa & wa, Vvj & vj, Vwj & wj) const {
          switch (LIN_ORDER)
          {
            case 3: vj = ublas::project(x, iaAbsJerk);
            case 2: va = ublas::project(x, iaAbsAcc);
            case 1: v = ublas::project(x, iaAbsVel);
            case 0: p = ublas::project(x, iaPos);
          }
          switch (ANG_ORDER)
          {
            case 3: wj = ublas::project(x, iaAngJerk);
            case 2: wa = ublas::project(x, iaAngAcc);
            case 1: w = ublas::project(x, iaAngVel);
            case 0: q = ublas::project(x, iaOriQuat);
          }
        }


        template<class Vp, class Vq, class Vv, class Vw,
                class Vva, class Vwa, class Vvj, class Vwj, class Vx>
        inline void unsplitState(const Vp & p, const Vq & q, const Vv & v, const Vw & w,
          const Vva & va, const Vwa & wa, const Vvj & vj, const Vwj & wj, Vx & x) const {
          switch (LIN_ORDER)
          {
            case 3: ublas::project(x, iaAbsJerk) = vj;
            case 2: ublas::project(x, iaAbsAcc) = va;
            case 1: ublas::project(x, iaAbsVel) = v;
            case 0: ublas::project(x, iaPos) = p;
          }
          switch (ANG_ORDER)
          {
            case 3: ublas::project(x, iaAngJerk) = wj;
            case 2: ublas::project(x, iaAngAcc) = wa;
            case 1: ublas::project(x, iaAngVel) = w;
            case 0: ublas::project(x, iaOriQuat) = q;
          }
        }



        template<class Vu, class V>
        inline void splitControl(const Vu & u, V & vi, V & wi) const {
          vi = project(u, ublas::range(0, 3));
          wi = project(u, ublas::range(3, 6));
        }

      private:
        // temporary matrices to speed up Jacobian computation
        struct TempVariables
        {
          mat43 QWDT_wdt; 
          mat44 QNEW_qwdt; 
          mat43 QNEW_wdt; 
          mat44 QNEW_q; 
          mat44 QNORM_qnew; 
        };
        mutable TempVariables tempvars0, tempvars1;

    };

  }
}

#endif /* ROBOTCONSTANTMOTIONMODEL_HPP_ */