robotInertial.hpp

Go to the documentation of this file.

#ifndef ROBOTINERTIAL_HPP_
#define ROBOTINERTIAL_HPP_

#include "jmath/jblas.hpp"
#include "boost/shared_ptr.hpp"
#include "rtslam/robotAbstract.hpp"

#define USE_NEW_STATE 1

#define INIT_Q_FROM_G 1

#define ESTIMATE_G_SIZE 0

#define ESTIMATE_BIASES 3

#define CONTROL_DETERMINISTIC 0


#if INIT_Q_FROM_G==0 && ESTIMATE_G_SIZE!=3
#error "Incompatible options INIT_Q_FROM_G and ESTIMATE_BIASES"
#endif

namespace jafar {
  namespace rtslam {

    class RobotInertial;
    typedef boost::shared_ptr<RobotInertial> robinertial_ptr_t;


    class RobotInertial: public RobotAbstract {
      private:
        static int g_size;
        double g_norm;
        double g_uncert;
        jblas::vec3 z_axis;
        ublas::range iaPos, iaOriQuat, iaAbsVel, iaBiasA, iaBiasW, iaG;

      public:

        RobotInertial(const map_ptr_t & _mapPtr);

        ~RobotInertial() {
        }

        virtual std::string typeName() const {
          return "Inertial";
        }

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

        virtual void move_func() {
          vec x = state.x();
          vec n = perturbation.x();
          vec xnew;
          move_func(x, control, n, dt_or_dx, x, XNEW_x, XNEW_pert);
          state.x() = xnew;
        }

        static size_t size() {
          return 10
          #if ESTIMATE_BIASES&1
          +3
          #endif
          #if ESTIMATE_BIASES&2
          +3
          #endif
          +g_size;
        }

        static size_t size_control() {
          return 6;
        }

        static size_t size_perturbation() {
          return 6
          #if ESTIMATE_BIASES // perturbation is 12 even if only ab or ww is estimated, for simplification purpose
          +6
          #endif
          ;
        }

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

        // Set initial uncertainties on linear velocity and gravity.
        void setInitialStd(double velLinStd, double aBiasStd, double wBiasStd, double gravStd){
          for (size_t i = iaAbsVel.start(); i < iaAbsVel.start()+iaAbsVel.size(); i++){
            state.P(i,i) = velLinStd*velLinStd;
          }
          #if ESTIMATE_BIASES&1
          for (size_t i = iaBiasA.start(); i < iaBiasA.start()+iaBiasA.size(); i++){
            state.P(i,i) = aBiasStd*aBiasStd;
          }
          #endif
          #if ESTIMATE_BIASES&2
          for (size_t i = iaBiasW.start(); i < iaBiasW.start()+iaBiasW.size(); i++){
            state.P(i,i) = wBiasStd*wBiasStd;
          }
          #endif
          if (g_size != 0)
            for (size_t i = iaG.start(); i < iaG.start()+iaG.size(); i++){
              #if !INIT_Q_FROM_G
              if (gravStd < 3) { gravStd = 6.0; std::cout << "Warning: gravity must be free to move, changing stdev to 6.0" << std::endl; }
              #endif
              state.P(i,i) = gravStd*gravStd;
            }
          else
            g_uncert = gravStd;
        }

        void setInitialParams(double gNorm)
        {
          g_norm = gNorm;
        }

        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 Vab, class Vwb, class Vg>
        inline void splitState(const Vx & x, Vp & p, Vq & q, Vv & v, Vab & ab, Vwb & wb, Vg & g) const {
          p = ublas::project(x, iaPos);
          q = ublas::project(x, iaOriQuat);
          v = ublas::project(x, iaAbsVel);
          #if ESTIMATE_BIASES&1
          ab = ublas::project(x, iaBiasA);
          #else
          ab.clear();
          #endif
          #if ESTIMATE_BIASES&2
          wb = ublas::project(x, iaBiasW);
          #else
          wb.clear();
          #endif
          if (g_size != 0)
            g = ublas::project(x, iaG);
          else
            { g.resize(1); g(0) = -g_norm; }
        }


        template<class Vp, class Vq, class Vv, class Vab, class Vwb, class Vg, class Vx>
        inline void unsplitState(const Vp & p, const Vq & q, const Vv & v, const Vab & ab, const Vwb & wb, const Vg & g, Vx & x) const {
          ublas::project(x, iaPos) = p;
          ublas::project(x, iaOriQuat) = q;
          ublas::project(x, iaAbsVel) = v;
          #if ESTIMATE_BIASES&1
          ublas::project(x, iaBiasA) = ab;
          #endif
          #if ESTIMATE_BIASES&2
          ublas::project(x, iaBiasW) = wb;
          #endif
          if (g_size != 0)
            ublas::project(x, iaG) = g;
        }


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

        template<class Vn, class V>
        inline void splitPert(const Vn & n, V & an, V & wn, V & ar, V & wr) const {
          an = project(n, ublas::range(0, 3));
          wn = project(n, ublas::range(3, 6));
          #if ESTIMATE_BIASES&1
          ar = project(n, ublas::range(6, 9));
          #else
          ar.clear();
          #endif
          #if ESTIMATE_BIASES&2
          wr = project(n, ublas::range(9, 12));
          #else
          wr.clear();
          #endif
        }

        vec e_from_g(const vec3 & _g);

      private:
        // Temporary members to accelerate Jacobian computation
        struct TempVariables
        {
          mat33 Idt; 
          mat33 Rdt; 
          mat44 QN_q; 
          mat44 QN_qwdt; 
          mat44 QN_w; 
          mat43 QWDT_w; 
          mat34 VNEW_q; 
          mat44 QNEW_qn; 
          mat43 QNEW_w; 
        };
        mutable TempVariables tempvars0, tempvars1;

    };

  }
}

#endif /* ROBOTINERTIAL_HPP_ */