diff --git a/src/capture_imu.cpp b/src/capture_imu.cpp
index 66cfc4d50ce5c9140da58639e4526ce237b97303..166841416a5098452d7914505c758a898633e1fb 100644
--- a/src/capture_imu.cpp
+++ b/src/capture_imu.cpp
@@ -11,7 +11,7 @@ CaptureIMU::CaptureIMU(const TimeStamp& _init_ts, const TimeStamp& _final_ts, Se
 }
 
 CaptureIMU::CaptureIMU(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr,
-                             const Eigen::Vector6s& _data, const Eigen::Matrix<WolfScalar,6,3>& _data_covariance) :
+                             const Eigen::Vector6s& _data, const Eigen::Matrix<Scalar,6,3>& _data_covariance) :
         CaptureMotion(_init_ts, _final_ts, _sensor_ptr, _data, _data_covariance)
 {
     //
diff --git a/src/capture_imu.h b/src/capture_imu.h
index 1e35f2ed734c724c8bdf72ea860189d351c6cdd5..7e0dccaa6c738bf0fe6ab205e0956820cb6e9372 100644
--- a/src/capture_imu.h
+++ b/src/capture_imu.h
@@ -14,7 +14,7 @@ class CaptureIMU : public CaptureMotion
 
         CaptureIMU(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr, const Eigen::Vector6s& _data);
 
-        CaptureIMU(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr, const Eigen::Vector6s& _data, const Eigen::Matrix<WolfScalar,6,3>& _data_covariance);
+        CaptureIMU(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr, const Eigen::Vector6s& _data, const Eigen::Matrix<Scalar,6,3>& _data_covariance);
 
         /** \brief Default destructor (not recommended)
          *
diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp
index fc86a3264d1d7805b5d13e4846d1e932b07a7c1e..a111ecbd60538240e319188ee223a1c305419380 100644
--- a/src/capture_odom_2D.cpp
+++ b/src/capture_odom_2D.cpp
@@ -98,7 +98,7 @@ void CaptureOdom2D::integrateCapture(CaptureMotion* _new_capture)
 
 CaptureOdom2D* CaptureOdom2D::interpolateCapture(const TimeStamp& _ts)
 {
-    WolfScalar ratio = (_ts.get() - this->time_stamp_.get()) / (this->final_time_stamp_.get() - this->time_stamp_.get());
+    Scalar ratio = (_ts.get() - this->time_stamp_.get()) / (this->final_time_stamp_.get() - this->time_stamp_.get());
 
     // Second part
     CaptureOdom2D* second_odom_ptr = new CaptureOdom2D(_ts, final_time_stamp_, sensor_ptr_, data_ * (1-ratio), data_covariance_ * (1-ratio));
diff --git a/src/ceres_wrapper/auto_diff_cost_function_wrapper.h b/src/ceres_wrapper/auto_diff_cost_function_wrapper.h
index a1e7e71c1cdb11821a61512462a9cae59fa7befa..151b77747df5308e1b96664bdfd95bee9ea9a4da 100644
--- a/src/ceres_wrapper/auto_diff_cost_function_wrapper.h
+++ b/src/ceres_wrapper/auto_diff_cost_function_wrapper.h
@@ -17,7 +17,7 @@ class AutoDiffCostFunctionWrapperBase : public ceres::SizedCostFunction<MEASUREM
                                                              BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,
                                                              BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE>
 {
-    typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE +
+    typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE +
                                    BLOCK_5_SIZE + BLOCK_6_SIZE + BLOCK_7_SIZE + BLOCK_8_SIZE + BLOCK_9_SIZE> WolfJet;
 
     protected:
@@ -164,7 +164,7 @@ class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE,
                                BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,
                                BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,0>
 {
-    typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE +
+    typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE +
                                    BLOCK_5_SIZE + BLOCK_6_SIZE + BLOCK_7_SIZE + BLOCK_8_SIZE> WolfJet;
 
     protected:
@@ -302,7 +302,7 @@ class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE,
                                BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,
                                BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,0,0>
 {
-    typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE +
+    typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE +
                                    BLOCK_5_SIZE + BLOCK_6_SIZE + BLOCK_7_SIZE> WolfJet;
 
     protected:
@@ -433,7 +433,7 @@ class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE,
                                BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,
                                BLOCK_5_SIZE,BLOCK_6_SIZE,0,0,0>
 {
-    typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE +
+    typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE +
                                    BLOCK_5_SIZE + BLOCK_6_SIZE> WolfJet;
 
     protected:
@@ -557,7 +557,7 @@ class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE,
                                BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,
                                BLOCK_5_SIZE,0,0,0,0>
 {
-    typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE +
+    typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE +
                                    BLOCK_5_SIZE> WolfJet;
 
     protected:
@@ -672,7 +672,7 @@ class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE,
                                BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,
                                0,0,0,0,0>
 {
-    typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE> WolfJet;
+    typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE> WolfJet;
 
     protected:
         ConstraintType* constraint_ptr_;
@@ -778,7 +778,7 @@ class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE,
                                BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,0,
                                0,0,0,0,0>
 {
-    typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE> WolfJet;
+    typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE> WolfJet;
 
     protected:
         ConstraintType* constraint_ptr_;
@@ -874,7 +874,7 @@ class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE,
                                BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,0,0,
                                0,0,0,0,0>
 {
-    typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE> WolfJet;
+    typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE> WolfJet;
 
     protected:
         ConstraintType* constraint_ptr_;
@@ -965,7 +965,7 @@ class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE,
                                BLOCK_0_SIZE,BLOCK_1_SIZE,0,0,0,
                                0,0,0,0,0>
 {
-    typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE + BLOCK_1_SIZE> WolfJet;
+    typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE> WolfJet;
 
     protected:
         ConstraintType* constraint_ptr_;
@@ -1048,7 +1048,7 @@ class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE,
                                BLOCK_0_SIZE,0,0,0,0,
                                0,0,0,0,0>
 {
-    typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE> WolfJet;
+    typedef ceres::Jet<Scalar, BLOCK_0_SIZE> WolfJet;
 
     protected:
         ConstraintType* constraint_ptr_;
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 9ff7999f5af5bc0fcf068b8637491398e9bf87a9..271130cf9f679918c5673edd7e3d5464df304a4d 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -155,7 +155,7 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
         // STORE DESIRED COVARIANCES
         for (unsigned int i = 0; i < double_pairs.size(); i++)
         {
-            Eigen::Matrix<WolfScalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize());
+            Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize());
             covariance_->GetCovarianceBlock(double_pairs[i].first, double_pairs[i].second, cov.data());
             //std::cout << "getted covariance " << std::endl << cov << std::endl;
             wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
diff --git a/src/constraint_analytic.cpp b/src/constraint_analytic.cpp
index 148ac34d7ef15291eb547e3165fd8411ff6057ce..726af57ea4775158125bf9897a025df3d4d91298 100644
--- a/src/constraint_analytic.cpp
+++ b/src/constraint_analytic.cpp
@@ -51,7 +51,7 @@ ConstraintAnalytic::~ConstraintAnalytic()
 }
 
 
-const std::vector<WolfScalar*> ConstraintAnalytic::getStateBlockPtrVector()
+const std::vector<Scalar*> ConstraintAnalytic::getStateBlockPtrVector()
 {
     assert(state_ptr_vector_.size() > 0 && state_ptr_vector_.size() <= 10 && "Wrong state vector size in constraint, it should be between 1 and 10");
 
@@ -59,29 +59,29 @@ const std::vector<WolfScalar*> ConstraintAnalytic::getStateBlockPtrVector()
     {
         case 1:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr()});
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr()});
         }
         case 2:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
                                              state_ptr_vector_[1]->getPtr()});
         }
         case 3:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
                                              state_ptr_vector_[1]->getPtr(),
                                              state_ptr_vector_[2]->getPtr()});
         }
         case 4:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
                                              state_ptr_vector_[1]->getPtr(),
                                              state_ptr_vector_[2]->getPtr(),
                                              state_ptr_vector_[3]->getPtr()});
         }
         case 5:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
                                              state_ptr_vector_[1]->getPtr(),
                                              state_ptr_vector_[2]->getPtr(),
                                              state_ptr_vector_[3]->getPtr(),
@@ -89,7 +89,7 @@ const std::vector<WolfScalar*> ConstraintAnalytic::getStateBlockPtrVector()
         }
         case 6:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
                                              state_ptr_vector_[1]->getPtr(),
                                              state_ptr_vector_[2]->getPtr(),
                                              state_ptr_vector_[3]->getPtr(),
@@ -98,7 +98,7 @@ const std::vector<WolfScalar*> ConstraintAnalytic::getStateBlockPtrVector()
         }
         case 7:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
                                              state_ptr_vector_[1]->getPtr(),
                                              state_ptr_vector_[2]->getPtr(),
                                              state_ptr_vector_[3]->getPtr(),
@@ -108,7 +108,7 @@ const std::vector<WolfScalar*> ConstraintAnalytic::getStateBlockPtrVector()
         }
         case 8:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
                                              state_ptr_vector_[1]->getPtr(),
                                              state_ptr_vector_[2]->getPtr(),
                                              state_ptr_vector_[3]->getPtr(),
@@ -119,7 +119,7 @@ const std::vector<WolfScalar*> ConstraintAnalytic::getStateBlockPtrVector()
         }
         case 9:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
                                              state_ptr_vector_[1]->getPtr(),
                                              state_ptr_vector_[2]->getPtr(),
                                              state_ptr_vector_[3]->getPtr(),
@@ -131,7 +131,7 @@ const std::vector<WolfScalar*> ConstraintAnalytic::getStateBlockPtrVector()
         }
         case 10:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
                                              state_ptr_vector_[1]->getPtr(),
                                              state_ptr_vector_[2]->getPtr(),
                                              state_ptr_vector_[3]->getPtr(),
@@ -144,7 +144,7 @@ const std::vector<WolfScalar*> ConstraintAnalytic::getStateBlockPtrVector()
         }
     }
 
-    return std::vector<WolfScalar*>(0); //Not going to happen
+    return std::vector<Scalar*>(0); //Not going to happen
 }
 
 const std::vector<StateBlock*> ConstraintAnalytic::getStatePtrVector() const
diff --git a/src/constraint_analytic.h b/src/constraint_analytic.h
index 1620d6e66de6253a1ad44020c3fb624e4c39df8b..ee87df853fdeeb823f3659c0f8add8d291156c08 100644
--- a/src/constraint_analytic.h
+++ b/src/constraint_analytic.h
@@ -96,7 +96,7 @@ class ConstraintAnalytic: public ConstraintBase
          * Returns a vector of pointers to the state blocks in which this constraint depends
          *
          **/
-        virtual const std::vector<WolfScalar*> getStateBlockPtrVector();
+        virtual const std::vector<Scalar*> getStateBlockPtrVector();
 
         /** \brief Returns a vector of pointers to the states
          *
diff --git a/src/constraint_base.h b/src/constraint_base.h
index 735830ea27140fe8890fe04ac3156d74e3d75870..8dc4b96ee081b0a9d205154ff22af0d0d9b301e7 100644
--- a/src/constraint_base.h
+++ b/src/constraint_base.h
@@ -62,7 +62,7 @@ class ConstraintBase : public NodeLinked<FeatureBase, NodeTerminus>
 
         /** \brief Returns a vector of scalar pointers to the first element of all state blocks involved in the constraint
          **/
-        virtual const std::vector<WolfScalar*> getStateBlockPtrVector() = 0;
+        virtual const std::vector<Scalar*> getStateBlockPtrVector() = 0;
 
         /** \brief Returns a vector of pointers to the states in which this constraint depends
          **/
diff --git a/src/constraint_container.h b/src/constraint_container.h
index c92162372baa8351d1f59573e1b2027043b69d6f..b590e75d0452781ec62c559523579670c4eaa78b 100644
--- a/src/constraint_container.h
+++ b/src/constraint_container.h
@@ -59,7 +59,7 @@ class ConstraintContainer: public ConstraintSparse<3,2,1,2,1>
 			// sensor transformation
 			Eigen::Matrix<T,2,1> sensor_position = getCapturePtr()->getSensorPtr()->getPPtr()->getVector().head(2).cast<T>();
 //			Eigen::Matrix<T,2,2> inverse_R_sensor = (getCapturePtr()->getSensorPtr()->getRotationMatrix2D().transpose()).cast<T>();
-                        Eigen::Rotation2D<WolfScalar> S_R( getCapturePtr()->getSensorOPtr()->getVector()(0) );
+                        Eigen::Rotation2D<Scalar> S_R( getCapturePtr()->getSensorOPtr()->getVector()(0) );
                         Eigen::Matrix<T,2,2> inverse_R_sensor = (S_R.matrix().transpose()).cast<T>();
 
 
diff --git a/src/constraint_corner_2D.h b/src/constraint_corner_2D.h
index e73d6dab2219792716f35c9f16f9975fc0fe9547..7c5f7c3af4acf76e77022a3c329ad429a3c6ec06 100644
--- a/src/constraint_corner_2D.h
+++ b/src/constraint_corner_2D.h
@@ -65,7 +65,7 @@ inline bool ConstraintCorner2D::operator ()(const T* const _robotP, const T* con
     // sensor transformation
     Eigen::Matrix<T, 2, 1> sensor_position = getCapturePtr()->getSensorPtr()->getPPtr()->getVector().head(2).cast<T>();
     //                        Eigen::Matrix<T,2,2> inverse_R_sensor = (getCapturePtr()->getSensorPtr()->getRotationMatrix2D().transpose()).cast<T>();
-    Eigen::Rotation2D<WolfScalar> S_R(getCapturePtr()->getSensorOPtr()->getVector()(0));
+    Eigen::Rotation2D<Scalar> S_R(getCapturePtr()->getSensorOPtr()->getVector()(0));
     Eigen::Matrix<T, 2, 2> inverse_R_sensor = (S_R.matrix().transpose()).cast<T>();
     // robot information
     Eigen::Matrix<T, 2, 2> inverse_R_robot;
diff --git a/src/constraint_epipolar.h b/src/constraint_epipolar.h
index 1cb1e4416f1ff51cedf87b175def972343c77809..67834f596d913eebeacfd29d98ce8a83546f632e 100644
--- a/src/constraint_epipolar.h
+++ b/src/constraint_epipolar.h
@@ -18,7 +18,7 @@ class ConstraintEpipolar : public ConstraintBase
 
         /** \brief Returns a vector of scalar pointers to the first element of all state blocks involved in the constraint
          **/
-        virtual const std::vector<WolfScalar*> getStateBlockPtrVector(){return std::vector<WolfScalar*>(0);}
+        virtual const std::vector<Scalar*> getStateBlockPtrVector(){return std::vector<Scalar*>(0);}
 
         /** \brief Returns a vector of pointers to the states in which this constraint depends
          **/
diff --git a/src/constraint_gps_pseudorange_2D.h b/src/constraint_gps_pseudorange_2D.h
index 4c28b6e998f0167f4666c771317ca687e01662e5..9c07bb8aa6292a35b803eaaaf823b28e1895e794 100644
--- a/src/constraint_gps_pseudorange_2D.h
+++ b/src/constraint_gps_pseudorange_2D.h
@@ -72,7 +72,7 @@ public:
 
 protected:
     Eigen::Vector3s sat_position_;
-    WolfScalar pseudorange_;
+    Scalar pseudorange_;
 
 };
 
diff --git a/src/constraint_gps_pseudorange_3D.h b/src/constraint_gps_pseudorange_3D.h
index 5e7b22ad8b26b6d12fc4b3e2cc0e5efa4e453e76..7d3e6a0b203b832186d9cbbb72dc42a7b63803ce 100644
--- a/src/constraint_gps_pseudorange_3D.h
+++ b/src/constraint_gps_pseudorange_3D.h
@@ -71,7 +71,7 @@ public:
 
 protected:
     Eigen::Vector3s sat_position_;
-    WolfScalar pseudorange_;
+    Scalar pseudorange_;
 
 };
 
diff --git a/src/constraint_odom_2D_analytic.h b/src/constraint_odom_2D_analytic.h
index b45ee9c824c8c6486de5de47efad1d177a6fbe9b..7591b269372b3f77fd72679858f2b6ba03718713 100644
--- a/src/constraint_odom_2D_analytic.h
+++ b/src/constraint_odom_2D_analytic.h
@@ -47,7 +47,7 @@ class ConstraintOdom2DAnalytic : public ConstraintRelative2DAnalytic
 //            Eigen::VectorXs expected_measurement(3);
 //
 //            // Expected measurement
-//            Eigen::Matrix2s R = Eigen::Rotation2D<WolfScalar>(-_st_vector[1](0)).matrix();
+//            Eigen::Matrix2s R = Eigen::Rotation2D<Scalar>(-_st_vector[1](0)).matrix();
 //            expected_measurement.head(2) = R * (_st_vector[2]-_st_vector[0]); // rotar menys l'angle de primer (-_o1)
 //            expected_measurement(2) = _st_vector[3](0) - _st_vector[1](0);
 //
diff --git a/src/constraint_relative_2D_analytic.h b/src/constraint_relative_2D_analytic.h
index 1f51f61a2132c3de3d3b1156b71568bbd6f9a03d..92a81a2d3c918645461dc217cb2ff5950a53df21 100644
--- a/src/constraint_relative_2D_analytic.h
+++ b/src/constraint_relative_2D_analytic.h
@@ -117,7 +117,7 @@ inline Eigen::VectorXs ConstraintRelative2DAnalytic::evaluateResiduals(
     Eigen::VectorXs residual(3);
     Eigen::VectorXs expected_measurement(3);
     // Expected measurement
-    Eigen::Matrix2s R = Eigen::Rotation2D<WolfScalar>(-_st_vector[1](0)).matrix();
+    Eigen::Matrix2s R = Eigen::Rotation2D<Scalar>(-_st_vector[1](0)).matrix();
     expected_measurement.head(2) = R * (_st_vector[2] - _st_vector[0]); // rotar menys l'angle de primer (-_o1)
     expected_measurement(2) = _st_vector[3](0) - _st_vector[1](0);
     // Residual
diff --git a/src/constraint_sparse.h b/src/constraint_sparse.h
index 497927ff23bc3951c77bc9503d94ae85cafa23cd..02e9339c6f1bc1dd118ddfba33ede097cac8c52f 100644
--- a/src/constraint_sparse.h
+++ b/src/constraint_sparse.h
@@ -121,7 +121,7 @@ class ConstraintSparse: public ConstraintBase
          * Returns a vector of pointers to the state blocks in which this constraint depends
          *
          **/
-        virtual const std::vector<WolfScalar*> getStateBlockPtrVector();
+        virtual const std::vector<Scalar*> getStateBlockPtrVector();
 
         /** \brief Returns a vector of pointers to the states
          *
@@ -338,7 +338,7 @@ template <const unsigned int MEASUREMENT_SIZE,
                 unsigned int BLOCK_7_SIZE,
                 unsigned int BLOCK_8_SIZE,
                 unsigned int BLOCK_9_SIZE>
-const std::vector<WolfScalar*> ConstraintSparse<MEASUREMENT_SIZE,
+const std::vector<Scalar*> ConstraintSparse<MEASUREMENT_SIZE,
                                                 BLOCK_0_SIZE,
                                                 BLOCK_1_SIZE,
                                                 BLOCK_2_SIZE,
@@ -356,29 +356,29 @@ const std::vector<WolfScalar*> ConstraintSparse<MEASUREMENT_SIZE,
     {
         case 1:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr()});
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr()});
         }
         case 2:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
                                              state_ptr_vector_[1]->getPtr()});
         }
         case 3:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
                                              state_ptr_vector_[1]->getPtr(),
                                              state_ptr_vector_[2]->getPtr()});
         }
         case 4:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
                                              state_ptr_vector_[1]->getPtr(),
                                              state_ptr_vector_[2]->getPtr(),
                                              state_ptr_vector_[3]->getPtr()});
         }
         case 5:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
                                              state_ptr_vector_[1]->getPtr(),
                                              state_ptr_vector_[2]->getPtr(),
                                              state_ptr_vector_[3]->getPtr(),
@@ -386,7 +386,7 @@ const std::vector<WolfScalar*> ConstraintSparse<MEASUREMENT_SIZE,
         }
         case 6:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
                                              state_ptr_vector_[1]->getPtr(),
                                              state_ptr_vector_[2]->getPtr(),
                                              state_ptr_vector_[3]->getPtr(),
@@ -395,7 +395,7 @@ const std::vector<WolfScalar*> ConstraintSparse<MEASUREMENT_SIZE,
         }
         case 7:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
                                              state_ptr_vector_[1]->getPtr(),
                                              state_ptr_vector_[2]->getPtr(),
                                              state_ptr_vector_[3]->getPtr(),
@@ -405,7 +405,7 @@ const std::vector<WolfScalar*> ConstraintSparse<MEASUREMENT_SIZE,
         }
         case 8:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
                                              state_ptr_vector_[1]->getPtr(),
                                              state_ptr_vector_[2]->getPtr(),
                                              state_ptr_vector_[3]->getPtr(),
@@ -416,7 +416,7 @@ const std::vector<WolfScalar*> ConstraintSparse<MEASUREMENT_SIZE,
         }
         case 9:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
                                              state_ptr_vector_[1]->getPtr(),
                                              state_ptr_vector_[2]->getPtr(),
                                              state_ptr_vector_[3]->getPtr(),
@@ -428,7 +428,7 @@ const std::vector<WolfScalar*> ConstraintSparse<MEASUREMENT_SIZE,
         }
         case 10:
         {
-            return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
                                              state_ptr_vector_[1]->getPtr(),
                                              state_ptr_vector_[2]->getPtr(),
                                              state_ptr_vector_[3]->getPtr(),
@@ -441,7 +441,7 @@ const std::vector<WolfScalar*> ConstraintSparse<MEASUREMENT_SIZE,
         }
     }
 
-    return std::vector<WolfScalar*>(0); //Not going to happen
+    return std::vector<Scalar*>(0); //Not going to happen
 }
 
 template <const unsigned int MEASUREMENT_SIZE,
diff --git a/src/examples/solver/test_iQR_wolf2.cpp b/src/examples/solver/test_iQR_wolf2.cpp
index 9ee5dc67617d24ac39baf2d79b31754df277496f..173229b1b03a2016b0f888c3be0790536a077d35 100644
--- a/src/examples/solver/test_iQR_wolf2.cpp
+++ b/src/examples/solver/test_iQR_wolf2.cpp
@@ -120,10 +120,10 @@ int main(int argc, char *argv[])
 
 
     //init random generators
-    WolfScalar odom_std_factor = 0.1;
-    WolfScalar gps_std = 10;
+    Scalar odom_std_factor = 0.1;
+    Scalar gps_std = 10;
     std::default_random_engine generator(1);
-    std::normal_distribution<WolfScalar> gaussian_distribution(0.0, 1);
+    std::normal_distribution<Scalar> gaussian_distribution(0.0, 1);
 
     // Faramotics stuff
     Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
@@ -300,7 +300,7 @@ int main(int argc, char *argv[])
         std::vector<double> landmark_vector;
         for (auto landmark_it = wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++)
         {
-            WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
+            Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
             landmark_vector.push_back(*position_ptr); //x
             landmark_vector.push_back(*(position_ptr + 1)); //y
             landmark_vector.push_back(0.2); //z
@@ -355,7 +355,7 @@ int main(int argc, char *argv[])
     std::vector<double> landmark_vector;
     for (auto landmark_it = wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++)
     {
-        WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
+        Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
         landmark_vector.push_back(*position_ptr); //x
         landmark_vector.push_back(*(position_ptr + 1)); //y
         landmark_vector.push_back(0.2); //z
diff --git a/src/examples/test_analytic_odom_constraint.cpp b/src/examples/test_analytic_odom_constraint.cpp
index fcc7a71b40e1bf172b3d3a719b9ef7b39f88d90c..f945d8c62333969d3de4a7ddace4be7c33852675 100644
--- a/src/examples/test_analytic_odom_constraint.cpp
+++ b/src/examples/test_analytic_odom_constraint.cpp
@@ -20,10 +20,10 @@
 
 namespace wolf {
 // inserts the sparse matrix 'ins' into the sparse matrix 'original' in the place given by 'row' and 'col' integers
-void insertSparseBlock(const Eigen::SparseMatrix<WolfScalar>& ins, Eigen::SparseMatrix<WolfScalar>& original, const unsigned int& row, const unsigned int& col)
+void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatrix<Scalar>& original, const unsigned int& row, const unsigned int& col)
 {
   for (int k=0; k<ins.outerSize(); ++k)
-    for (Eigen::SparseMatrix<WolfScalar>::InnerIterator iti(ins,k); iti; ++iti)
+    for (Eigen::SparseMatrix<Scalar>::InnerIterator iti(ins,k); iti; ++iti)
       original.coeffRef(iti.row() + row, iti.col() + col) = iti.value();
 
   original.makeCompressed();
diff --git a/src/examples/test_autodiff.cpp b/src/examples/test_autodiff.cpp
index aae3d005ee426444d856f7beb76e9dd4b3a87cef..e7dd1649541216df66432f15c9e11da601eef934 100644
--- a/src/examples/test_autodiff.cpp
+++ b/src/examples/test_autodiff.cpp
@@ -73,11 +73,11 @@ int main(int argc, char** argv)
 
     // INITIALIZATION ============================================================================================
     //init random generators
-    WolfScalar odom_std_factor = 0.5;
-    WolfScalar gps_std = 1;
+    Scalar odom_std_factor = 0.5;
+    Scalar gps_std = 1;
     std::default_random_engine generator(1);
-    std::normal_distribution<WolfScalar> distribution_odom(0.0, odom_std_factor); //odometry noise
-    std::normal_distribution<WolfScalar> distribution_gps(0.0, gps_std); //GPS noise
+    std::normal_distribution<Scalar> distribution_odom(0.0, odom_std_factor); //odometry noise
+    std::normal_distribution<Scalar> distribution_gps(0.0, gps_std); //GPS noise
 
     //init google log
     //google::InitGoogleLogging(argv[0]);
@@ -295,7 +295,7 @@ int main(int argc, char** argv)
 //        std::vector<double> landmark_vector;
 //        for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++)
 //        {
-//            WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
+//            Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
 //            landmark_vector.push_back(*position_ptr); //x
 //            landmark_vector.push_back(*(position_ptr + 1)); //y
 //            landmark_vector.push_back(0.2); //z
@@ -348,7 +348,7 @@ int main(int argc, char** argv)
 //    std::vector<double> landmark_vector;
 //    for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++)
 //    {
-//        WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
+//        Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
 //        landmark_vector.push_back(*position_ptr); //x
 //        landmark_vector.push_back(*(position_ptr + 1)); //y
 //        landmark_vector.push_back(0.2); //z
diff --git a/src/examples/test_capture_laser_2D.cpp b/src/examples/test_capture_laser_2D.cpp
index 64318628898f470bc9e65b951064d0590bb9f3fe..5d64d279e7e4723aafda319bef5e52a88a5a6d05 100644
--- a/src/examples/test_capture_laser_2D.cpp
+++ b/src/examples/test_capture_laser_2D.cpp
@@ -101,7 +101,7 @@ int main(int argc, char *argv[])
     
     //init a noise generator
     std::default_random_engine generator(1);
-    std::normal_distribution<WolfScalar> distribution_range(0.,device.getRangeStdDev()); //odometry noise
+    std::normal_distribution<Scalar> distribution_range(0.,device.getRangeStdDev()); //odometry noise
     
     //Create a Capture object
     CaptureLaser2D capture(time_stamp, &device, ranges);
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index a1b5c86c8302e1fbc566d568ca321483b0c30d42..99797393a659507101fe30dd64a0164b1a703066 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -72,11 +72,11 @@ int main(int argc, char** argv)
 
     // INITIALIZATION ============================================================================================
     //init random generators
-    WolfScalar odom_std_factor = 0.1;
-    WolfScalar gps_std = 1;
+    Scalar odom_std_factor = 0.1;
+    Scalar gps_std = 1;
     std::default_random_engine generator(1);
-    std::normal_distribution<WolfScalar> distribution_odom(0.0, odom_std_factor); //odometry noise
-    std::normal_distribution<WolfScalar> distribution_gps(0.0, gps_std); //GPS noise
+    std::normal_distribution<Scalar> distribution_odom(0.0, odom_std_factor); //odometry noise
+    std::normal_distribution<Scalar> distribution_gps(0.0, gps_std); //GPS noise
 
     //init google log
     //google::InitGoogleLogging(argv[0]);
@@ -248,7 +248,7 @@ int main(int argc, char** argv)
         std::vector<double> landmark_vector;
         for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++)
         {
-            WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
+            Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
             landmark_vector.push_back(*position_ptr); //x
             landmark_vector.push_back(*(position_ptr + 1)); //y
             landmark_vector.push_back(0.2); //z
@@ -301,7 +301,7 @@ int main(int argc, char** argv)
     std::vector<double> landmark_vector;
     for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++)
     {
-        WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
+        Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
         landmark_vector.push_back(*position_ptr); //x
         landmark_vector.push_back(*(position_ptr + 1)); //y
         landmark_vector.push_back(0.2); //z
diff --git a/src/examples/test_eigen_quaternion.cpp b/src/examples/test_eigen_quaternion.cpp
index 9eca8b910e5717e44fb920114afc550e8dff47fa..cea2324871950a1cf754430a2435449a8e061bb9 100644
--- a/src/examples/test_eigen_quaternion.cpp
+++ b/src/examples/test_eigen_quaternion.cpp
@@ -14,7 +14,7 @@ int main()
 
     std::cout << std::endl << "Eigen Quatenrnion test" << std::endl;
     
-    WolfScalar q1[4]; 
+    Scalar q1[4]; 
     Eigen::Map<Eigen::Quaternions> q1_map(q1);
     
     //try to find out how eigen sorts storage (real part tail or head ? )
diff --git a/src/examples/test_image.cpp b/src/examples/test_image.cpp
index 0919eea9056c6e7bafc96fca7995ea1c9a6c0f82..575ed417ae0e87ac6fe58ce71e051c2a575d7bd1 100644
--- a/src/examples/test_image.cpp
+++ b/src/examples/test_image.cpp
@@ -141,7 +141,7 @@ int main(int argc, char** argv)
         capture >> frame;
 
         if (f>1){ // check if consecutive images are different
-            WolfScalar diff = cv::norm(frame, last_frame, cv::NORM_L1);
+            Scalar diff = cv::norm(frame, last_frame, cv::NORM_L1);
             std::cout << "test_image: Image increment: " << diff << std::endl;
         }
 
diff --git a/src/examples/test_motion.cpp b/src/examples/test_motion.cpp
index 43fec51c48044143c72dcde338a72d362068d814..139a7d8424c4fe5ca98bbc98b3e4dc2ca165f904 100644
--- a/src/examples/test_motion.cpp
+++ b/src/examples/test_motion.cpp
@@ -31,7 +31,7 @@ int main()
     TimeStamp t0, t;
     t0.setToNow();
     t = t0;
-    WolfScalar dt = .01;
+    Scalar dt = .01;
 
     // robot state blocks: origin
     StateBlock sb_pos(Eigen::Vector3s::Zero());
@@ -75,7 +75,7 @@ int main()
     std::cout << "\nQuery states at asynchronous time values..." << std::endl;
 
     t = t0;
-    WolfScalar dt_2 = dt/2;
+    Scalar dt_2 = dt/2;
     dt = 0.0045; // new dt
     for (int i = 1; i <= 20; i++)
     {
@@ -91,15 +91,15 @@ int main()
 
     std::cout << "\n\nTrying a std::map as the buffer container <-- NOT WORKING: need exact key" << std::endl;
 
-    WolfScalar x;
-    std::map<TimeStamp, WolfScalar> buffer_map;
+    Scalar x;
+    std::map<TimeStamp, Scalar> buffer_map;
     t.set(0);
     x = 0;
     for (double i = 1; i<=10; i++)
     {
         t.set(i/5);
         x++;
-        buffer_map.insert(std::pair<TimeStamp,WolfScalar>(t,x));
+        buffer_map.insert(std::pair<TimeStamp,Scalar>(t,x));
         std::cout << "insert (ts,x) = (" << t.get() << "," << x << ")" << std::endl;
     }
     for (double i = 1; i<=8; i++)
@@ -114,7 +114,7 @@ int main()
 
     std::cout << "\n\nTrying a std::list and std::find_if as the buffer container <-- WORKING: can use comparator '<' for evaluating key" << std::endl;
 
-    typedef std::pair<TimeStamp, WolfScalar> Pair;
+    typedef std::pair<TimeStamp, Scalar> Pair;
     typedef std::list<Pair> PairsList;
 
     PairsList buffer_list;
diff --git a/src/examples/test_wolf_imported_graph.cpp b/src/examples/test_wolf_imported_graph.cpp
index b0bc70a6b390ecf20f008435959ed638e69fb572..5f33e257bf32095278a622ec3c3d085185825589 100644
--- a/src/examples/test_wolf_imported_graph.cpp
+++ b/src/examples/test_wolf_imported_graph.cpp
@@ -20,10 +20,10 @@
 
 namespace wolf{
 // inserts the sparse matrix 'ins' into the sparse matrix 'original' in the place given by 'row' and 'col' integers
-void insertSparseBlock(const Eigen::SparseMatrix<WolfScalar>& ins, Eigen::SparseMatrix<WolfScalar>& original, const unsigned int& row, const unsigned int& col)
+void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatrix<Scalar>& original, const unsigned int& row, const unsigned int& col)
 {
   for (int k=0; k<ins.outerSize(); ++k)
-    for (Eigen::SparseMatrix<WolfScalar>::InnerIterator iti(ins,k); iti; ++iti)
+    for (Eigen::SparseMatrix<Scalar>::InnerIterator iti(ins,k); iti; ++iti)
       original.coeffRef(iti.row() + row, iti.col() + col) = iti.value();
 
   original.makeCompressed();
@@ -54,7 +54,7 @@ int main(int argc, char** argv)
     clock_t t1;
     ceres::Solver::Summary summary_full, summary_prun;
     Eigen::MatrixXs Sigma_ii(3,3), Sigma_ij(3,3), Sigma_jj(3,3), Sigma_z(3,3), Ji(3,3), Jj(3,3);
-    WolfScalar xi, yi, thi, si, ci, xj, yj;
+    Scalar xi, yi, thi, si, ci, xj, yj;
 
     // loading variables
     std::map<unsigned int, FrameBase*> index_2_frame_ptr_full;
@@ -66,7 +66,7 @@ int main(int argc, char** argv)
     WolfProblem* wolf_problem_prun = new WolfProblem(FRM_PO_2D);
     SensorBase* sensor = new SensorBase(SEN_ODOM_2D, new StateBlock(Eigen::VectorXs::Zero(2)), new StateBlock(Eigen::VectorXs::Zero(1)), new StateBlock(Eigen::VectorXs::Zero(2)), 2);
 
-    Eigen::SparseMatrix<WolfScalar> Lambda(0,0);
+    Eigen::SparseMatrix<Scalar> Lambda(0,0);
 
     // Ceres wrapper
     ceres::Solver::Options ceres_options;
@@ -275,13 +275,13 @@ int main(int argc, char** argv)
                     //std::cout << "information " << std::endl << edge_information << std::endl;
                     //std::cout << "covariance " << std::endl << constraint_ptr_prun->getMeasurementCovariance() << std::endl;
 
-                    WolfScalar xi = *(frame_old_ptr_prun->getPPtr()->getPtr());
-                    WolfScalar yi = *(frame_old_ptr_prun->getPPtr()->getPtr()+1);
-                    WolfScalar thi = *(frame_old_ptr_prun->getOPtr()->getPtr());
-                    WolfScalar si = sin(thi);
-                    WolfScalar ci = cos(thi);
-                    WolfScalar xj = *(frame_new_ptr_prun->getPPtr()->getPtr());
-                    WolfScalar yj = *(frame_new_ptr_prun->getPPtr()->getPtr()+1);
+                    Scalar xi = *(frame_old_ptr_prun->getPPtr()->getPtr());
+                    Scalar yi = *(frame_old_ptr_prun->getPPtr()->getPtr()+1);
+                    Scalar thi = *(frame_old_ptr_prun->getOPtr()->getPtr());
+                    Scalar si = sin(thi);
+                    Scalar ci = cos(thi);
+                    Scalar xj = *(frame_new_ptr_prun->getPPtr()->getPtr());
+                    Scalar yj = *(frame_new_ptr_prun->getPPtr()->getPtr()+1);
                     Eigen::MatrixXs Ji(3,3), Jj(3,3);
                     Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci,
                            si,-ci,-(xj-xi)*ci-(yj-yi)*si,
@@ -291,7 +291,7 @@ int main(int argc, char** argv)
                             0,  0, 1;
                     //std::cout << "Ji" << std::endl << Ji << std::endl;
                     //std::cout << "Jj" << std::endl << Jj << std::endl;
-                    Eigen::SparseMatrix<WolfScalar> DeltaLambda(Lambda.rows(), Lambda.cols());
+                    Eigen::SparseMatrix<Scalar> DeltaLambda(Lambda.rows(), Lambda.cols());
                     insertSparseBlock((Ji.transpose() * edge_information * Ji).sparseView(), DeltaLambda, edge_old*3, edge_old*3);
                     insertSparseBlock((Jj.transpose() * edge_information * Jj).sparseView(), DeltaLambda, edge_new*3, edge_new*3);
                     insertSparseBlock((Ji.transpose() * edge_information * Jj).sparseView(), DeltaLambda, edge_old*3, edge_new*3);
@@ -317,7 +317,7 @@ int main(int argc, char** argv)
     initial_covariance_full->process();
     initial_covariance_prun->process();
     //std::cout << "initial covariance: constraint " << initial_covariance_prun->getFeatureListPtr()->front()->getConstraintFromListPtr()->front()->nodeId() << std::endl << initial_covariance_prun->getFeatureListPtr()->front()->getMeasurementCovariance() << std::endl;
-    Eigen::SparseMatrix<WolfScalar> DeltaLambda(Lambda.rows(), Lambda.cols());
+    Eigen::SparseMatrix<Scalar> DeltaLambda(Lambda.rows(), Lambda.cols());
     insertSparseBlock((Eigen::Matrix3s::Identity() * 100).sparseView(), DeltaLambda, 0, 0);
     Lambda = Lambda + DeltaLambda;
 
@@ -332,7 +332,7 @@ int main(int argc, char** argv)
     wolf_problem_prun->getTrajectoryPtr()->getConstraintList(constraints);
     // Manual covariance computation
     t1 = clock();
-    Eigen::SimplicialLLT<Eigen::SparseMatrix<WolfScalar>> chol(Lambda);  // performs a Cholesky factorization of A
+    Eigen::SimplicialLLT<Eigen::SparseMatrix<Scalar>> chol(Lambda);  // performs a Cholesky factorization of A
     Eigen::MatrixXs Sigma = chol.solve(Eigen::MatrixXs::Identity(Lambda.rows(), Lambda.cols()));
     double t_sigma_manual = ((double) clock() - t1) / CLOCKS_PER_SEC;
     //std::cout << "Lambda" << std::endl << Lambda << std::endl;
@@ -398,7 +398,7 @@ int main(int argc, char** argv)
 
         //std::cout << "denominador : " << std::endl << Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose()) << std::endl;
         // Information gain
-        WolfScalar IG = 0.5 * log( Sigma_z.determinant() / (Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose())).determinant() );
+        Scalar IG = 0.5 * log( Sigma_z.determinant() / (Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose())).determinant() );
         //std::cout << "IG = " << IG << std::endl;
 
         if (IG < 2)
diff --git a/src/examples/test_wolf_prunning.cpp b/src/examples/test_wolf_prunning.cpp
index 7d21eb97a9760e55d28dff5b97c3cd7454247f6e..e92064788fc3740bce8d237f857fc59413f21f5b 100644
--- a/src/examples/test_wolf_prunning.cpp
+++ b/src/examples/test_wolf_prunning.cpp
@@ -21,10 +21,10 @@
 namespace wolf{
 // inserts the sparse matrix 'ins' into the sparse matrix 'original' in the place given by 'row' and 'col' integers
 
-void insertSparseBlock(const Eigen::SparseMatrix<WolfScalar>& ins, Eigen::SparseMatrix<WolfScalar>& original, const unsigned int& row, const unsigned int& col)
+void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatrix<Scalar>& original, const unsigned int& row, const unsigned int& col)
 {
   for (int k=0; k<ins.outerSize(); ++k)
-    for (Eigen::SparseMatrix<WolfScalar>::InnerIterator iti(ins,k); iti; ++iti)
+    for (Eigen::SparseMatrix<Scalar>::InnerIterator iti(ins,k); iti; ++iti)
       original.coeffRef(iti.row() + row, iti.col() + col) = iti.value();
 
   original.makeCompressed();
@@ -65,7 +65,7 @@ int main(int argc, char** argv)
     jacobians.push_back(Eigen::MatrixXs::Zero(3,1));
     jacobians.push_back(Eigen::MatrixXs::Zero(3,2));
     jacobians.push_back(Eigen::MatrixXs::Zero(3,1));
-    WolfScalar xi, yi, thi, si, ci, xj, yj;
+    Scalar xi, yi, thi, si, ci, xj, yj;
     double t_sigma_manual = 0;
 
     // loading variables
@@ -78,11 +78,11 @@ int main(int argc, char** argv)
     WolfProblem* wolf_problem_prun = new WolfProblem(FRM_PO_2D);
     SensorBase* sensor = new SensorBase(SEN_ODOM_2D, new StateBlock(Eigen::VectorXs::Zero(2)), new StateBlock(Eigen::VectorXs::Zero(1)), new StateBlock(Eigen::VectorXs::Zero(2)), 2);
 
-    Eigen::SparseMatrix<WolfScalar> Lambda(0,0);
+    Eigen::SparseMatrix<Scalar> Lambda(0,0);
 
     // prunning
     std::list<ConstraintBase*> ordered_ctr_ptr;
-    std::list<WolfScalar> ordered_ig;
+    std::list<Scalar> ordered_ig;
 
     // Ceres wrapper
     ceres::Solver::Options ceres_options;
@@ -291,13 +291,13 @@ int main(int argc, char** argv)
                     //std::cout << "covariance " << std::endl << constraint_ptr_prun->getMeasurementCovariance() << std::endl;
 
                     t1 = clock();
-                    WolfScalar xi = *(frame_old_ptr_prun->getPPtr()->getPtr());
-                    WolfScalar yi = *(frame_old_ptr_prun->getPPtr()->getPtr()+1);
-                    WolfScalar thi = *(frame_old_ptr_prun->getOPtr()->getPtr());
-                    WolfScalar si = sin(thi);
-                    WolfScalar ci = cos(thi);
-                    WolfScalar xj = *(frame_new_ptr_prun->getPPtr()->getPtr());
-                    WolfScalar yj = *(frame_new_ptr_prun->getPPtr()->getPtr()+1);
+                    Scalar xi = *(frame_old_ptr_prun->getPPtr()->getPtr());
+                    Scalar yi = *(frame_old_ptr_prun->getPPtr()->getPtr()+1);
+                    Scalar thi = *(frame_old_ptr_prun->getOPtr()->getPtr());
+                    Scalar si = sin(thi);
+                    Scalar ci = cos(thi);
+                    Scalar xj = *(frame_new_ptr_prun->getPPtr()->getPtr());
+                    Scalar yj = *(frame_new_ptr_prun->getPPtr()->getPtr()+1);
                     Eigen::MatrixXs Ji(3,3), Jj(3,3);
                     Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci,
                            si,-ci,-(xj-xi)*ci-(yj-yi)*si,
@@ -307,7 +307,7 @@ int main(int argc, char** argv)
                             0,  0, 1;
                     //std::cout << "Ji" << std::endl << Ji << std::endl;
                     //std::cout << "Jj" << std::endl << Jj << std::endl;
-                    Eigen::SparseMatrix<WolfScalar> DeltaLambda(Lambda.rows(), Lambda.cols());
+                    Eigen::SparseMatrix<Scalar> DeltaLambda(Lambda.rows(), Lambda.cols());
                     insertSparseBlock((Ji.transpose() * edge_information * Ji).sparseView(), DeltaLambda, edge_old*3, edge_old*3);
                     insertSparseBlock((Jj.transpose() * edge_information * Jj).sparseView(), DeltaLambda, edge_new*3, edge_new*3);
                     insertSparseBlock((Ji.transpose() * edge_information * Jj).sparseView(), DeltaLambda, edge_old*3, edge_new*3);
@@ -335,7 +335,7 @@ int main(int argc, char** argv)
     initial_covariance_prun->process();
     //std::cout << "initial covariance: constraint " << initial_covariance_prun->getFeatureListPtr()->front()->getConstraintFromListPtr()->front()->nodeId() << std::endl << initial_covariance_prun->getFeatureListPtr()->front()->getMeasurementCovariance() << std::endl;
     t1 = clock();
-    Eigen::SparseMatrix<WolfScalar> DeltaLambda(Lambda.rows(), Lambda.cols());
+    Eigen::SparseMatrix<Scalar> DeltaLambda(Lambda.rows(), Lambda.cols());
     insertSparseBlock((Eigen::Matrix3s::Identity() * 100).sparseView(), DeltaLambda, 0, 0);
     Lambda = Lambda + DeltaLambda;
     t_sigma_manual += ((double) clock() - t1) / CLOCKS_PER_SEC;
@@ -351,7 +351,7 @@ int main(int argc, char** argv)
     wolf_problem_prun->getTrajectoryPtr()->getConstraintList(constraints);
     // Manual covariance computation
     t1 = clock();
-    Eigen::SimplicialLLT<Eigen::SparseMatrix<WolfScalar>> chol(Lambda);  // performs a Cholesky factorization of A
+    Eigen::SimplicialLLT<Eigen::SparseMatrix<Scalar>> chol(Lambda);  // performs a Cholesky factorization of A
     Eigen::MatrixXs Sigma = chol.solve(Eigen::MatrixXs::Identity(Lambda.rows(), Lambda.cols()));
     t_sigma_manual += ((double) clock() - t1) / CLOCKS_PER_SEC;
     //std::cout << "Lambda" << std::endl << Lambda << std::endl;
@@ -424,7 +424,7 @@ int main(int argc, char** argv)
 //        std::cout << "J4" << std::endl << J4 << std::endl;
 
         // Information gain
-        WolfScalar IG_new = 0.5 * log( Sigma_z.determinant() /
+        Scalar IG_new = 0.5 * log( Sigma_z.determinant() /
                                  ( Sigma_z - (J1 * Sigma_11 * J1.transpose() +
                                               J1 * Sigma_12 * J2.transpose() +
                                               J1 * Sigma_13 * J3.transpose() +
@@ -504,7 +504,7 @@ int main(int argc, char** argv)
 
         //std::cout << "denominador : " << std::endl << Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose()) << std::endl;
         // Information gain
-        WolfScalar IG = 0.5 * log( Sigma_z.determinant() / (Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose())).determinant() );
+        Scalar IG = 0.5 * log( Sigma_z.determinant() / (Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose())).determinant() );
 
 //        std::cout << "part = " << std::endl << (Ji * Sigma_ii * Ji.transpose() +
 //                                                Jj * Sigma_jj * Jj.transpose() +
diff --git a/src/feature_base.h b/src/feature_base.h
index ef0fe032b5138839c08c482f6952916bf71028b1..c1505fce202fe9206be3b5c2debd3dcde4f20e63 100644
--- a/src/feature_base.h
+++ b/src/feature_base.h
@@ -70,7 +70,7 @@ class FeatureBase : public NodeConstrained<CaptureBase,ConstraintBase>
          * 
          * WARNING: To be fast, it does not check that index _ii is smaller than dimension.
          **/
-        WolfScalar getMeasurement(unsigned int _ii) const;
+        Scalar getMeasurement(unsigned int _ii) const;
 
         /** \brief Returns a reference to the feature measurement covariance
          **/
@@ -91,7 +91,7 @@ inline CaptureBase* FeatureBase::getCapturePtr() const
     return upperNodePtr();
 }
 
-inline WolfScalar FeatureBase::getMeasurement(unsigned int _ii) const
+inline Scalar FeatureBase::getMeasurement(unsigned int _ii) const
 {
     return measurement_(_ii);
 }
diff --git a/src/feature_corner_2D.cpp b/src/feature_corner_2D.cpp
index ce4a025e13ff76d52e2e17baaa4b7d490cdf63a9..37c8c4c3c3979c6fb9f8835535917e63a4d020fd 100644
--- a/src/feature_corner_2D.cpp
+++ b/src/feature_corner_2D.cpp
@@ -14,7 +14,7 @@ FeatureCorner2D::~FeatureCorner2D()
     //
 }
 
-WolfScalar FeatureCorner2D::getAperture() const
+Scalar FeatureCorner2D::getAperture() const
 {
     return measurement_(3);
 }
diff --git a/src/feature_corner_2D.h b/src/feature_corner_2D.h
index cab19ab23aec20004759d27277a7cc9761a865a8..8e72baf91a0a4bad125ca34fb60f0f2af87359f0 100644
--- a/src/feature_corner_2D.h
+++ b/src/feature_corner_2D.h
@@ -18,7 +18,7 @@ class FeatureCorner2D : public FeatureBase
          * 
          * constructor
          */
-        FeatureCorner2D(const Eigen::Vector4s & _measurement, const Eigen::Matrix4s & _meas_covariance); //TODO: add const WolfScalar& aperture);
+        FeatureCorner2D(const Eigen::Vector4s & _measurement, const Eigen::Matrix4s & _meas_covariance); //TODO: add const Scalar& aperture);
 
         /** \brief Default destructor (not recommended)
          *
@@ -32,7 +32,7 @@ class FeatureCorner2D : public FeatureBase
          * Returns aperture
          * 
          **/
-        WolfScalar getAperture() const; 
+        Scalar getAperture() const; 
         
 };
 
diff --git a/src/feature_gps_pseudorange.cpp b/src/feature_gps_pseudorange.cpp
index 096038437f27d1140a6798930c568bb1204b45d0..4042b4f3d285c5cc369b7faae5a3e99a191a1a40 100644
--- a/src/feature_gps_pseudorange.cpp
+++ b/src/feature_gps_pseudorange.cpp
@@ -2,7 +2,7 @@
 
 namespace wolf {
 
-FeatureGPSPseudorange::FeatureGPSPseudorange(Eigen::Vector3s &_sat_position, WolfScalar _pseudorange, WolfScalar _covariance) :
+FeatureGPSPseudorange::FeatureGPSPseudorange(Eigen::Vector3s &_sat_position, Scalar _pseudorange, Scalar _covariance) :
         FeatureBase(FEAT_GPS_PR, Eigen::VectorXs::Constant(1,_pseudorange), Eigen::MatrixXs::Identity(1,1)*_covariance),
         sat_position_(_sat_position),
         pseudorange_(_pseudorange)
@@ -19,7 +19,7 @@ FeatureGPSPseudorange::~FeatureGPSPseudorange()
 
 }
 
-WolfScalar FeatureGPSPseudorange::getPseudorange() const
+Scalar FeatureGPSPseudorange::getPseudorange() const
 {
     return pseudorange_;
 }
diff --git a/src/feature_gps_pseudorange.h b/src/feature_gps_pseudorange.h
index 922ad001d57ecd4f26ea1f852f34f5f6108eaaaa..431efb32ab5a47db507d7c36a0a5d2f02107b2cc 100644
--- a/src/feature_gps_pseudorange.h
+++ b/src/feature_gps_pseudorange.h
@@ -18,10 +18,10 @@ class FeatureGPSPseudorange : public FeatureBase
 {
 protected:
     Eigen::Vector3s sat_position_;
-    WolfScalar pseudorange_;
+    Scalar pseudorange_;
 
 public:
-    FeatureGPSPseudorange(Eigen::Vector3s& _sat_position, WolfScalar _pseudorange, WolfScalar _covariance);
+    FeatureGPSPseudorange(Eigen::Vector3s& _sat_position, Scalar _pseudorange, Scalar _covariance);
 
     /** \brief Default destructor (not recommended)
      *
@@ -32,7 +32,7 @@ public:
 
     const Eigen::Vector3s & getSatPosition() const;
 
-    WolfScalar getPseudorange() const;
+    Scalar getPseudorange() const;
 
 };
 
diff --git a/src/feature_point_image.h b/src/feature_point_image.h
index 35c80eca6123cbccb52a81680af58e34582faa0f..8ae3546441c8f09ebe40c3c9256f977e94871d58 100644
--- a/src/feature_point_image.h
+++ b/src/feature_point_image.h
@@ -63,8 +63,8 @@ class FeaturePointImage : public FeatureBase
         void setIsKnown(bool _is_known);
 
         Eigen::VectorXs & getMeasurement(){
-            measurement_(0) = WolfScalar(keypoint_.pt.x);
-            measurement_(1) = WolfScalar(keypoint_.pt.y);
+            measurement_(0) = Scalar(keypoint_.pt.x);
+            measurement_(1) = Scalar(keypoint_.pt.y);
             return measurement_;
         }
 
diff --git a/src/landmark_base.h b/src/landmark_base.h
index 59481577d7d725d7fdb87c0af20ed009b3349fe4..e7fd9e4e8da91bf1b85298becdda67bbd66111d5 100644
--- a/src/landmark_base.h
+++ b/src/landmark_base.h
@@ -98,7 +98,7 @@ class LandmarkBase : public NodeConstrained<MapBase, NodeTerminus>
          * 
          * WARNING: To be fast, it does not check that index _ii is smaller than dimension.
          **/
-        WolfScalar getDescriptor(unsigned int _ii) const;
+        Scalar getDescriptor(unsigned int _ii) const;
 
         /** \brief Return the type of the landmark
          **/
@@ -149,7 +149,7 @@ inline void LandmarkBase::setDescriptor(const Eigen::VectorXs& _descriptor)
     descriptor_ = _descriptor;
 }
 
-inline WolfScalar LandmarkBase::getDescriptor(unsigned int _ii) const
+inline Scalar LandmarkBase::getDescriptor(unsigned int _ii) const
 {
     return descriptor_(_ii);
 }
diff --git a/src/landmark_container.cpp b/src/landmark_container.cpp
index 3c70c794c72882157b58d044f31cf264f4fffc52..0bd54d60b42e19f5fbd16abc71c11898102acd8a 100644
--- a/src/landmark_container.cpp
+++ b/src/landmark_container.cpp
@@ -4,7 +4,7 @@
 
 namespace wolf {
 
-LandmarkContainer::LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const WolfScalar& _witdh, const WolfScalar& _length) :
+LandmarkContainer::LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const Scalar& _witdh, const Scalar& _length) :
 	LandmarkBase(LANDMARK_CONTAINER, _p_ptr, _o_ptr),
 	corners_(3,4)
 {
@@ -17,7 +17,7 @@ LandmarkContainer::LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, con
                 M_PI / 4,     3 * M_PI / 4,-3 * M_PI / 4,-M_PI / 4;
 }
 
-LandmarkContainer::LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const WolfScalar& _witdh, const WolfScalar& _length) :
+LandmarkContainer::LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const Scalar& _witdh, const Scalar& _length) :
     LandmarkBase(LANDMARK_CONTAINER, _p_ptr, _o_ptr),
     corners_(3,4)
 {
@@ -83,7 +83,7 @@ LandmarkContainer::LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, con
     std::cout << "Container center pose... " << container_position.transpose() << " " << container_orientation.transpose() << std::endl;
 }
 
-//LandmarkContainer::LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const WolfScalar& _witdh, const WolfScalar& _length) :
+//LandmarkContainer::LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const Scalar& _witdh, const Scalar& _length) :
 //    LandmarkBase(LANDMARK_CONTAINER, _p_ptr, _o_ptr),
 //    corners_(3,4)
 //{
@@ -164,12 +164,12 @@ LandmarkContainer::~LandmarkContainer()
     // tODO delete corners
 }
 
-WolfScalar LandmarkContainer::getWidth() const
+Scalar LandmarkContainer::getWidth() const
 {
     return descriptor_(0);
 }
 
-WolfScalar LandmarkContainer::getLength() const
+Scalar LandmarkContainer::getLength() const
 {
     return descriptor_(1);
 }
diff --git a/src/landmark_container.h b/src/landmark_container.h
index 02bc307d15fda36e4fb41fd226931909dd644e25..869279a01cff9ba2deb131b708cc6261c48e4f3e 100644
--- a/src/landmark_container.h
+++ b/src/landmark_container.h
@@ -26,7 +26,7 @@ class LandmarkContainer : public LandmarkBase
          * \param _length descriptor of the landmark: container length
          *
          **/
-		LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const WolfScalar& _witdh=2.44, const WolfScalar& _length=12.2);
+		LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const Scalar& _witdh=2.44, const Scalar& _length=12.2);
 
 		/** \brief Constructor with type, time stamp and the position state pointer
 		 *
@@ -51,7 +51,7 @@ class LandmarkContainer : public LandmarkBase
          * \param _length descriptor of the landmark: container length
          *
          **/
-		LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const WolfScalar& _witdh=2.44, const WolfScalar& _length=12.2);
+		LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const Scalar& _witdh=2.44, const Scalar& _length=12.2);
 
         /** \brief Constructor with type, time stamp and the position state pointer
          *
@@ -66,7 +66,7 @@ class LandmarkContainer : public LandmarkBase
          * \param _length descriptor of the landmark: container length
          *
          **/
-		//LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const WolfScalar& _witdh=2.44, const WolfScalar& _length=12.2);
+		//LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const Scalar& _witdh=2.44, const Scalar& _length=12.2);
 
         /** \brief Default destructor (not recommended)
          *
@@ -80,14 +80,14 @@ class LandmarkContainer : public LandmarkBase
          * Returns the container width
          *
          **/
-        WolfScalar getWidth() const;
+        Scalar getWidth() const;
 
         /** \brief Returns the container length
          * 
          * Returns the container length
          * 
          **/
-        WolfScalar getLength() const;
+        Scalar getLength() const;
 
         /** \brief Returns the container corners in container coordinates
          *
diff --git a/src/landmark_corner_2D.cpp b/src/landmark_corner_2D.cpp
index f6b32607ce483ccaba35e5e96fe4f7b8ca20fc61..1a6f1eacaea17f08fbeb10226a7f0c0b19702712 100644
--- a/src/landmark_corner_2D.cpp
+++ b/src/landmark_corner_2D.cpp
@@ -3,7 +3,7 @@
 
 namespace wolf {
 
-LandmarkCorner2D::LandmarkCorner2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const WolfScalar& _aperture) :
+LandmarkCorner2D::LandmarkCorner2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const Scalar& _aperture) :
 	LandmarkBase(LANDMARK_CORNER, _p_ptr, _o_ptr)
 {
   	setDescriptor(Eigen::VectorXs::Constant(1,_aperture));
@@ -15,7 +15,7 @@ LandmarkCorner2D::~LandmarkCorner2D()
 }
 
 
-WolfScalar LandmarkCorner2D::getAperture() const
+Scalar LandmarkCorner2D::getAperture() const
 {
     return descriptor_(0);
 }
diff --git a/src/landmark_corner_2D.h b/src/landmark_corner_2D.h
index 573941bdb1b12f3c93e0643649daa3369d18fd03..73156a6e04ef13566cb1de65652eacba6de957a4 100644
--- a/src/landmark_corner_2D.h
+++ b/src/landmark_corner_2D.h
@@ -25,7 +25,7 @@ class LandmarkCorner2D : public LandmarkBase
          * \param _aperture descriptor of the landmark: aperture of the corner
          *
          **/
-		LandmarkCorner2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const WolfScalar& _aperture=0);
+		LandmarkCorner2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const Scalar& _aperture=0);
 
         /** \brief Default destructor (not recommended)
          *
@@ -39,7 +39,7 @@ class LandmarkCorner2D : public LandmarkBase
          * Returns aperture
          * 
          **/
-        WolfScalar getAperture() const;         
+        Scalar getAperture() const;         
         
 };
 
diff --git a/src/local_parametrization_homogeneous.cpp b/src/local_parametrization_homogeneous.cpp
index 329435599ef18bfc399d43060fd556cae7ae6a59..1df0975b4bb79483e1c5e79b1e115a9d39649272 100644
--- a/src/local_parametrization_homogeneous.cpp
+++ b/src/local_parametrization_homogeneous.cpp
@@ -37,7 +37,7 @@ bool LocalParametrizationHomogeneous::plus(const Eigen::Map<const Eigen::VectorX
         Vector3s axis = _delta / norm_delta;
 
         // express delta as a quaternion
-        Quaternions dq(AngleAxis<WolfScalar>(norm_delta, axis));
+        Quaternions dq(AngleAxis<Scalar>(norm_delta, axis));
 
         // result as a homogeneous point -- we use the quaternion product for keeping in the 4-sphere
         _h_plus_delta = (dq * Map<const Quaternions>(_h.data())).coeffs();
diff --git a/src/local_parametrization_quaternion.cpp b/src/local_parametrization_quaternion.cpp
index 2c18aefb2bf04977e516cfc417b069220c499869..dd9c22ec2b7fea541c7660f5340b7f51928f14be 100644
--- a/src/local_parametrization_quaternion.cpp
+++ b/src/local_parametrization_quaternion.cpp
@@ -32,7 +32,7 @@ bool LocalParametrizationQuaternion::plus(const Eigen::Map<const Eigen::VectorXs
         Vector3s axis = _delta_theta / angle;
 
         // express delta_theta as a quaternion using the angle-axis helper
-        Quaternions dq(AngleAxis<WolfScalar>(angle, axis));
+        Quaternions dq(AngleAxis<Scalar>(angle, axis));
 
         // result as a quaternion
         if (delta_reference_ == DQ_GLOBAL)
diff --git a/src/processor_base.h b/src/processor_base.h
index b285ebf9a5f1ab27616b226029874be30b928db1..a73dfc3c349e7390b0298042a16fc113f675b17f 100644
--- a/src/processor_base.h
+++ b/src/processor_base.h
@@ -41,7 +41,7 @@ class ProcessorBase : public NodeLinked<SensorBase, NodeTerminus>
 
         SensorBase* getSensorPtr();
 
-        //virtual void newKeyFrameCallback(FrameBase* _new_key_frame_ptr, const WolfScalar& _time_tolerance) = 0;
+        //virtual void newKeyFrameCallback(FrameBase* _new_key_frame_ptr, const Scalar& _time_tolerance) = 0;
 
     private:
         ProcessorType type_;
diff --git a/src/processor_brisk.cpp b/src/processor_brisk.cpp
index 18c9c7a925d90cda38749b9b0df531295c488f24..e3164095952f63230b0aeb15b29534db5e24e0fa 100644
--- a/src/processor_brisk.cpp
+++ b/src/processor_brisk.cpp
@@ -157,7 +157,7 @@ unsigned int ProcessorBrisk::trackFeatures(const FeatureBaseList& _feature_list_
     std::cout << "Number of features to track: " << _feature_list_in.size() << std::endl << std::endl;
     std::cout << "last*: " << last_ptr_ << " -- incoming*: " << incoming_ptr_ << std::endl;
 
-    WolfScalar diff = cv::norm(image_incoming_, image_last_, cv::NORM_L1);
+    Scalar diff = cv::norm(image_incoming_, image_last_, cv::NORM_L1);
     std::cout << "Distance between last and incoming images: " << diff << std::endl;
 
     for (auto feature_base_ptr : _feature_list_in)
@@ -201,7 +201,7 @@ unsigned int ProcessorBrisk::trackFeatures(const FeatureBaseList& _feature_list_
                 _feature_list_out.push_back(incoming_point_ptr);
 
                 _feature_matches[incoming_point_ptr] = FeatureMatch(feature_base_ptr,
-                                                                            1 - (WolfScalar)(cv_matches[0].distance)/512); //FIXME: 512 is the maximum HAMMING distance
+                                                                            1 - (Scalar)(cv_matches[0].distance)/512); //FIXME: 512 is the maximum HAMMING distance
 
             }
             for (unsigned int i = 0; i < candidate_keypoints.size(); i++) // TODO Arreglar todos los <= y -1 por < y nada.
diff --git a/src/processor_brisk.h b/src/processor_brisk.h
index 00d7f039a382d77df919e8ad8e52615f98d87171..cbc618f28595223295e7a60da9b93d81478fde8e 100644
--- a/src/processor_brisk.h
+++ b/src/processor_brisk.h
@@ -46,7 +46,7 @@ struct ImageTrackerParameters
         }descriptor;
         struct Matcher
         {
-                WolfScalar max_similarity_distance; ///< 0: perfect match; 1 or -1: awful match; out of [-1,1]: error
+                Scalar max_similarity_distance; ///< 0: perfect match; 1 or -1: awful match; out of [-1,1]: error
         }matcher;
         struct Adtive_search
         {
diff --git a/src/processor_gps.cpp b/src/processor_gps.cpp
index 47659402d86659343db026eaecb985f7f553e43c..c99b7721396435423c0e742fc37e1ee32ba7734f 100644
--- a/src/processor_gps.cpp
+++ b/src/processor_gps.cpp
@@ -40,7 +40,7 @@ void ProcessorGPS::process(CaptureBase* _capture_ptr)
     for(unsigned int i = 0; i < obs.measurements_.size(); ++i)
     {
         Eigen::Vector3s sat_pos = obs.measurements_[i].sat_position_;
-        WolfScalar pr = obs.measurements_[i].pseudorange_;
+        Scalar pr = obs.measurements_[i].pseudorange_;
 
         capture_gps_ptr_->addFeature(new FeatureGPSPseudorange(sat_pos, pr, gps_covariance_));
     }
diff --git a/src/processor_gps.h b/src/processor_gps.h
index 9c0ed0dc447c00bcf5e73ff5ebb957fd9b176327..5fe4a732b9b20155e68eaf74cf63774bb1e96368 100644
--- a/src/processor_gps.h
+++ b/src/processor_gps.h
@@ -21,7 +21,7 @@ class ProcessorGPS : public ProcessorBase
         //SensorGPS* sensor_gps_ptr_; //specific pointer to sensor gps object
         CaptureGPS* capture_gps_ptr_;
 
-        WolfScalar gps_covariance_;
+        Scalar gps_covariance_;
 
 
     public:
diff --git a/src/processor_laser_2D.cpp b/src/processor_laser_2D.cpp
index b54e1f9a7ba1907d5f4d0cfd3ca8ebde6c3e0637..9ca54258b08661ccfcd1e3794e41fdb5c410dc2c 100644
--- a/src/processor_laser_2D.cpp
+++ b/src/processor_laser_2D.cpp
@@ -87,10 +87,10 @@ void ProcessorLaser2D::createFeatures(std::list<laserscanutils::Corner> & _corne
         meas(3) = corner_it->aperture_;
 
         // TODO: maybe in line object?
-        WolfScalar L1 = corner_it->line_1_.length();
-        WolfScalar L2 = corner_it->line_2_.length();
-        WolfScalar cov_angle_line1 = 12 * corner_it->line_1_.error_ / (pow(L1,2) * (pow(corner_it->line_1_.np_,3)-pow(corner_it->line_1_.np_,2)));
-        WolfScalar cov_angle_line2 = 12 * corner_it->line_2_.error_ / (pow(L2,2) * (pow(corner_it->line_2_.np_,3)-pow(corner_it->line_2_.np_,2)));
+        Scalar L1 = corner_it->line_1_.length();
+        Scalar L2 = corner_it->line_2_.length();
+        Scalar cov_angle_line1 = 12 * corner_it->line_1_.error_ / (pow(L1,2) * (pow(corner_it->line_1_.np_,3)-pow(corner_it->line_1_.np_,2)));
+        Scalar cov_angle_line2 = 12 * corner_it->line_2_.error_ / (pow(L2,2) * (pow(corner_it->line_2_.np_,3)-pow(corner_it->line_2_.np_,2)));
 
 
         //init cov in corner coordinates
@@ -103,7 +103,7 @@ void ProcessorLaser2D::createFeatures(std::list<laserscanutils::Corner> & _corne
         //std::cout << cov_mat << std::endl;
 
         // Rotate covariance
-        R.topLeftCorner<2,2>() = Eigen::Rotation2D<WolfScalar>(corner_it->orientation_).matrix();
+        R.topLeftCorner<2,2>() = Eigen::Rotation2D<Scalar>(corner_it->orientation_).matrix();
         cov_mat.topLeftCorner<3,3>() = R.transpose() * cov_mat.topLeftCorner<3,3>() * R;
 
         //std::cout << "rotated covariance: " << std::endl;
@@ -119,11 +119,11 @@ void ProcessorLaser2D::establishConstraintsMHTree()
     if (capture_laser_ptr_->getFeatureListPtr()->size() != 0)
     {
         //local declarations
-        WolfScalar prob, dm2;
+        Scalar prob, dm2;
         unsigned int ii, jj;
         std::map<unsigned int, unsigned int> ft_lk_pairs;
         std::vector<bool> associated_mask;
-        WolfScalar sq24 = sqrt(2) / 4;
+        Scalar sq24 = sqrt(2) / 4;
         Eigen::MatrixXs corners(3,4);
                     // Center seen from corners (see LandmarkContainer.h)
                     // A                                         // B                                         // C                                         // D
@@ -187,7 +187,7 @@ void ProcessorLaser2D::establishConstraintsMHTree()
                     if (fabs(pi2pi(((FeatureCorner2D*)(*feature_it))->getAperture() - 3 * M_PI / 2)) < MAX_ACCEPTED_APERTURE_DIFF)
                     {
                         // Rotate corners (seen from feature coordinates)
-                        //rotated_corners.topRows(2) = Eigen::Rotation2D<WolfScalar>(expected_features[*landmark_it](2)).matrix() * corners.topRows(2);
+                        //rotated_corners.topRows(2) = Eigen::Rotation2D<Scalar>(expected_features[*landmark_it](2)).matrix() * corners.topRows(2);
                         rotated_corners = corners;
                         squared_mahalanobis_distances = computeSquaredMahalanobisDistances((*feature_it), expected_features[*landmark_it], expected_features_covs[*landmark_it], rotated_corners);
 
@@ -270,21 +270,21 @@ void ProcessorLaser2D::establishConstraintsMHTree()
 
         // Global transformation
         Eigen::Vector2s t_robot = capture_laser_ptr_->getFramePtr()->getPPtr()->getVector().head(2);
-        Eigen::Matrix2s R_robot = Eigen::Rotation2D<WolfScalar>(*(capture_laser_ptr_->getFramePtr()->getOPtr()->getPtr())).matrix();
-        WolfScalar robot_orientation = *(capture_laser_ptr_->getFramePtr()->getOPtr()->getPtr());
+        Eigen::Matrix2s R_robot = Eigen::Rotation2D<Scalar>(*(capture_laser_ptr_->getFramePtr()->getOPtr()->getPtr())).matrix();
+        Scalar robot_orientation = *(capture_laser_ptr_->getFramePtr()->getOPtr()->getPtr());
 
         // Sensor transformation
         Eigen::Vector2s t_sensor = getSensorPtr()->getPPtr()->getVector().head(2);
-        Eigen::Rotation2D<WolfScalar> S_R( getSensorPtr()->getOPtr()->getVector()(0) );
+        Eigen::Rotation2D<Scalar> S_R( getSensorPtr()->getOPtr()->getVector()(0) );
         Eigen::Matrix2s R_sensor = S_R.matrix();
-        WolfScalar sensor_orientation = *(capture_laser_ptr_->getSensorPtr()->getOPtr()->getPtr());
+        Scalar sensor_orientation = *(capture_laser_ptr_->getSensorPtr()->getOPtr()->getPtr());
 
         // NEW CORNERS
         for (auto it=new_corner_landmarks.begin(); it!=new_corner_landmarks.end();it++)
         {
             //get feature on sensor frame
             const Eigen::Vector2s& feature_position = (*it)->getMeasurement().head(2);
-            const WolfScalar& feature_orientation = (*it)->getMeasurement()(2);
+            const Scalar& feature_orientation = (*it)->getMeasurement()(2);
 
             //translate feature position and orientation to world (global)
             feature_global_pose.head(2) = R_robot * (R_sensor * feature_position + t_sensor) + t_robot;
@@ -298,7 +298,7 @@ void ProcessorLaser2D::establishConstraintsMHTree()
         {
             //get feature on sensor frame
             const Eigen::Vector2s& feature_position = new_container_landmarks.at(i).first->getMeasurement().head(2);
-            const WolfScalar& feature_orientation = new_container_landmarks.at(i).first->getMeasurement()(2);
+            const Scalar& feature_orientation = new_container_landmarks.at(i).first->getMeasurement()(2);
 
             //translate feature position and orientation to world (global)
             feature_global_pose.head(2) = R_robot * (R_sensor * feature_position + t_sensor) + t_robot;
@@ -319,18 +319,18 @@ void ProcessorLaser2D::computeExpectedFeature(LandmarkBase* _landmark_ptr, Eigen
     FrameBase* frame_ptr = capture_laser_ptr_->getFramePtr();
 
     Eigen::Vector2s p_robot = frame_ptr->getPPtr()->getVector();
-    WolfScalar      o_robot = *(frame_ptr->getOPtr()->getPtr());
+    Scalar      o_robot = *(frame_ptr->getOPtr()->getPtr());
 
     Eigen::Vector2s p_sensor = getSensorPtr()->getPPtr()->getVector().head(2);
-    WolfScalar      o_sensor = *(getSensorPtr()->getOPtr()->getPtr());
-    Eigen::Rotation2D<WolfScalar> S_R( getSensorPtr()->getOPtr()->getVector()(0) );
+    Scalar      o_sensor = *(getSensorPtr()->getOPtr()->getPtr());
+    Eigen::Rotation2D<Scalar> S_R( getSensorPtr()->getOPtr()->getVector()(0) );
     Eigen::Matrix2s R_sensor = S_R.matrix();
 
     Eigen::Vector2s p_landmark = _landmark_ptr->getPPtr()->getVector();
-    WolfScalar      o_landmark = *(_landmark_ptr->getOPtr()->getPtr());
+    Scalar      o_landmark = *(_landmark_ptr->getOPtr()->getPtr());
 
-    WolfScalar o_rs = o_robot + o_sensor; // Sum of theta_sensor and theta_robot
-    Eigen::Matrix2s R_sr = Eigen::Rotation2D<WolfScalar>(o_rs).matrix(); // Sum of theta_sensor and theta_robot
+    Scalar o_rs = o_robot + o_sensor; // Sum of theta_sensor and theta_robot
+    Eigen::Matrix2s R_sr = Eigen::Rotation2D<Scalar>(o_rs).matrix(); // Sum of theta_sensor and theta_robot
 
     Eigen::Vector2s p_robot_landmark = p_robot - p_landmark;
 
@@ -349,7 +349,7 @@ void ProcessorLaser2D::computeExpectedFeature(LandmarkBase* _landmark_ptr, Eigen
 
     // ------------------------ Sigma
     // JACOBIAN
-    Eigen::Matrix<WolfScalar, 3, 6> Jlr = Eigen::Matrix<WolfScalar, 3, 6>::Zero();
+    Eigen::Matrix<Scalar, 3, 6> Jlr = Eigen::Matrix<Scalar, 3, 6>::Zero();
     Jlr.block<2,2>(0,0) = -R_sr.transpose();
     Jlr(2,2) = -1;
     Jlr.block<2,2>(0,3) = R_sr.transpose();
@@ -383,22 +383,22 @@ Eigen::VectorXs ProcessorLaser2D::computeSquaredMahalanobisDistances(const Featu
     FrameBase* frame_ptr = capture_laser_ptr_->getFramePtr();
 
     Eigen::Vector2s p_robot = frame_ptr->getPPtr()->getVector();
-    WolfScalar      o_robot = *(frame_ptr->getOPtr()->getPtr());
+    Scalar      o_robot = *(frame_ptr->getOPtr()->getPtr());
 
     Eigen::Vector2s p_sensor = getSensorPtr()->getPPtr()->getVector().head(2);
-    WolfScalar      o_sensor = *(getSensorPtr()->getOPtr()->getPtr());
+    Scalar      o_sensor = *(getSensorPtr()->getOPtr()->getPtr());
 //    Eigen::Matrix2s R_sensor = getSensorPtr()->getRotationMatrix2D();
-    Eigen::Rotation2D<WolfScalar> S_R( getSensorPtr()->getOPtr()->getVector()(0) );
+    Eigen::Rotation2D<Scalar> S_R( getSensorPtr()->getOPtr()->getVector()(0) );
     Eigen::Matrix2s R_sensor = S_R.matrix();
 
     Eigen::Vector2s p_landmark = _landmark_ptr->getPPtr()->getVector();
-    WolfScalar      o_landmark = *(_landmark_ptr->getOPtr()->getPtr());
+    Scalar      o_landmark = *(_landmark_ptr->getOPtr()->getPtr());
 
     const Eigen::Vector2s& p_feature = _feature_ptr->getMeasurement().head(2);
-    const WolfScalar&      o_feature = _feature_ptr->getMeasurement()(2);
+    const Scalar&      o_feature = _feature_ptr->getMeasurement()(2);
 
-    WolfScalar o_rs = o_robot + o_sensor; // Sum of theta_sensor and theta_robot
-    Eigen::Matrix2s R_sr = Eigen::Rotation2D<WolfScalar>(o_rs).matrix(); // Sum of theta_sensor and theta_robot
+    Scalar o_rs = o_robot + o_sensor; // Sum of theta_sensor and theta_robot
+    Eigen::Matrix2s R_sr = Eigen::Rotation2D<Scalar>(o_rs).matrix(); // Sum of theta_sensor and theta_robot
     Eigen::Vector2s p_robot_landmark = p_robot - p_landmark;
 
     // ------------------------ d
@@ -415,7 +415,7 @@ Eigen::VectorXs ProcessorLaser2D::computeSquaredMahalanobisDistances(const Featu
 
     // ------------------------ Sigma_d
     // JACOBIAN
-    Eigen::Matrix<WolfScalar, 3, 6> Jlr = Eigen::Matrix<WolfScalar, 3, 6>::Zero();
+    Eigen::Matrix<Scalar, 3, 6> Jlr = Eigen::Matrix<Scalar, 3, 6>::Zero();
     Jlr.block<2,2>(0,0) = -R_sr.transpose();
     Jlr(2,2) = -1;
     Jlr.block<2,2>(0,3) = R_sr.transpose();
@@ -456,7 +456,7 @@ Eigen::VectorXs ProcessorLaser2D::computeSquaredMahalanobisDistances(const Featu
     assert(_mu.rows() == 3 && "mahalanobis distance with bad number of mu components");
 
     const Eigen::Vector2s& p_feature = _feature_ptr->getMeasurement().head(2);
-    const WolfScalar&      o_feature = _feature_ptr->getMeasurement()(2);
+    const Scalar&      o_feature = _feature_ptr->getMeasurement()(2);
 
     // ------------------------ d
     Eigen::Vector3s d;
@@ -491,7 +491,7 @@ bool ProcessorLaser2D::fitNewContainer(FeatureCorner2D* _corner_feature_ptr, Lan
                     && std::fabs(pi2pi(((LandmarkCorner2D*)(*landmark_it))->getAperture() + M_PI / 2)) < MAX_ACCEPTED_APERTURE_DIFF) // should be a corner
             {
                 //std::cout << "landmark " << (*landmark_it)->nodeId() << std::endl;
-                WolfScalar SQ2 = sqrt(2)/2;
+                Scalar SQ2 = sqrt(2)/2;
                 Eigen::MatrixXs corners_relative_positions(3,6);
                                               // FROM A (see LandmarkContainer.h)                                                        // FROM B
                                               // Large side           // Short side          // Diagonal                                 // Large side           // Short side          // Diagonal
@@ -499,7 +499,7 @@ bool ProcessorLaser2D::fitNewContainer(FeatureCorner2D* _corner_feature_ptr, Lan
                                              -SQ2 * CONTAINER_LENGTH, SQ2 * CONTAINER_WIDTH, SQ2 * (CONTAINER_WIDTH - CONTAINER_LENGTH), SQ2 * CONTAINER_LENGTH,-SQ2 * CONTAINER_WIDTH, SQ2 * (CONTAINER_LENGTH - CONTAINER_WIDTH),
                                               M_PI / 2,              -M_PI / 2,              M_PI,                                      -M_PI / 2,               M_PI / 2,              M_PI;
 
-                Eigen::Matrix2s R_feature = Eigen::Rotation2D<WolfScalar>(_corner_feature_ptr->getMeasurement()(2)).matrix();
+                Eigen::Matrix2s R_feature = Eigen::Rotation2D<Scalar>(_corner_feature_ptr->getMeasurement()(2)).matrix();
                 corners_relative_positions = - corners_relative_positions;
                 corners_relative_positions.topRows(2) = R_feature * corners_relative_positions.topRows(2);
 
@@ -510,7 +510,7 @@ bool ProcessorLaser2D::fitNewContainer(FeatureCorner2D* _corner_feature_ptr, Lan
 //                for (unsigned int i = 0; i < 6; i++)
 //                    std::cout << erfc( sqrt(squared_mahalanobis_distances(i)/2) ) << std::endl;
 
-                WolfScalar SMD_threshold = 8;
+                Scalar SMD_threshold = 8;
                 if (squared_mahalanobis_distances(0) < SMD_threshold ) //erfc( sqrt(squared_mahalanobis_distances(0)/2) ) > 0.8 )
                 {
                     std::cout << "   large side! prob =  " << erfc( sqrt(squared_mahalanobis_distances(0)/2.0)) << std::endl;
@@ -584,7 +584,7 @@ void ProcessorLaser2D::createCornerLandmark(FeatureCorner2D* _corner_ptr, const
     Sigma_robot.block<1,2>(2,0) = Sigma_robot.block<2,1>(0,2).transpose();
 
     Eigen::Matrix3s R_robot3D = Eigen::Matrix3s::Identity();
-    R_robot3D.block<2,2>(0,0) = Eigen::Rotation2D<WolfScalar>(*(capture_laser_ptr_->getFramePtr()->getOPtr()->getPtr())).matrix();
+    R_robot3D.block<2,2>(0,0) = Eigen::Rotation2D<Scalar>(*(capture_laser_ptr_->getFramePtr()->getOPtr()->getPtr())).matrix();
     Eigen::Matrix3s Sigma_landmark = Sigma_robot + R_robot3D.transpose() * _corner_ptr->getMeasurementCovariance().topLeftCorner<3,3>() * R_robot3D;
 
     getWolfProblem()->addCovarianceBlock(new_landmark->getPPtr(), new_landmark->getPPtr(), Sigma_landmark.topLeftCorner<2,2>());
diff --git a/src/processor_laser_2D.h b/src/processor_laser_2D.h
index 65b9334d6a076a15b7b0f73ae30881c99f6b6052..adf0465d7e597bee43332015db4f2b07549b523d 100644
--- a/src/processor_laser_2D.h
+++ b/src/processor_laser_2D.h
@@ -41,9 +41,9 @@ namespace wolf {
 
 
 //some consts.. TODO: this tuning params should be grouped in a struct and passed to the class from ros node, at constructor level
-const WolfScalar MAX_ACCEPTED_APERTURE_DIFF = 20.0*M_PI/180.; //20 degrees
-const WolfScalar CONTAINER_WIDTH = 2.44;
-const WolfScalar CONTAINER_LENGTH = 12.20;
+const Scalar MAX_ACCEPTED_APERTURE_DIFF = 20.0*M_PI/180.; //20 degrees
+const Scalar CONTAINER_WIDTH = 2.44;
+const Scalar CONTAINER_LENGTH = 12.20;
 
 
 
diff --git a/src/processor_motion.h b/src/processor_motion.h
index ab3fd5a59c851d2898cd5fd085f1c10fe19d979c..b5a48b770052e67aa243823dc5ed73dce79cef86 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -24,7 +24,7 @@ class ProcessorMotion : public ProcessorBase
 
         // This is the main public interface
     public:
-        ProcessorMotion(ProcessorType _tp, WolfScalar _dt, size_t _state_size, size_t _delta_size, size_t _data_size);
+        ProcessorMotion(ProcessorType _tp, Scalar _dt, size_t _state_size, size_t _delta_size, size_t _data_size);
         virtual ~ProcessorMotion();
 
         // Instructions to the processor:
@@ -118,7 +118,7 @@ class ProcessorMotion : public ProcessorBase
           *
           *  However, other more complicated relations are possible.
           */
-         virtual void data2delta(const Eigen::VectorXs& _data, const WolfScalar _dt, Eigen::VectorXs& _delta) = 0;
+         virtual void data2delta(const Eigen::VectorXs& _data, const Scalar _dt, Eigen::VectorXs& _delta) = 0;
 
         /** \brief composes a delta-state on top of a state
          * \param _x the initial state
@@ -173,7 +173,7 @@ class ProcessorMotion : public ProcessorBase
 
     protected:
         // helpers to avoid allocation
-        WolfScalar dt_; ///< Time step
+        Scalar dt_; ///< Time step
         Eigen::VectorXs x_; ///< state temporary
         Eigen::VectorXs delta_, delta_integrated_; ///< current delta and integrated deltas
         Eigen::VectorXs data_; ///< current data
@@ -181,7 +181,7 @@ class ProcessorMotion : public ProcessorBase
 };
 
 
-inline ProcessorMotion::ProcessorMotion(ProcessorType _tp, WolfScalar _dt, size_t _state_size, size_t _delta_size, size_t _data_size) :
+inline ProcessorMotion::ProcessorMotion(ProcessorType _tp, Scalar _dt, size_t _state_size, size_t _delta_size, size_t _data_size) :
         ProcessorBase(_tp), x_size_(_state_size), delta_size_(_delta_size), data_size_(_data_size),
         origin_ptr_(nullptr), last_ptr_(nullptr), incoming_ptr_(nullptr),
         dt_(_dt), x_(_state_size),
diff --git a/src/processor_odom_3D.h b/src/processor_odom_3D.h
index 1b76f7edfaef8d6b76b0ebeebc6e4e03b80601df..732feebad9cc34c62a87b64831053b48e2681d9b 100644
--- a/src/processor_odom_3D.h
+++ b/src/processor_odom_3D.h
@@ -38,9 +38,9 @@ namespace wolf {
 class ProcessorOdom3d : public ProcessorMotion
 {
     public:
-        ProcessorOdom3d(WolfScalar _delta_t = 0);
+        ProcessorOdom3d(Scalar _delta_t = 0);
         virtual ~ProcessorOdom3d();
-        virtual void data2delta(const Eigen::VectorXs& _data, const WolfScalar _dt, Eigen::VectorXs& _delta);
+        virtual void data2delta(const Eigen::VectorXs& _data, const Scalar _dt, Eigen::VectorXs& _delta);
 
     protected:
         virtual void preProcess(){}
@@ -62,7 +62,7 @@ class ProcessorOdom3d : public ProcessorMotion
 };
 
 
-inline ProcessorOdom3d::ProcessorOdom3d(WolfScalar _delta_t) :
+inline ProcessorOdom3d::ProcessorOdom3d(Scalar _delta_t) :
         ProcessorMotion(PRC_ODOM_3D, _delta_t, 7, 7, 6),
         p1_(nullptr), //, q1_(nullptr)
         p2_(nullptr), //, q1_(nullptr)
@@ -77,7 +77,7 @@ inline ProcessorOdom3d::~ProcessorOdom3d()
 {
 }
 
-inline void ProcessorOdom3d::data2delta(const Eigen::VectorXs& _data, const WolfScalar _dt, Eigen::VectorXs& _delta)
+inline void ProcessorOdom3d::data2delta(const Eigen::VectorXs& _data, const Scalar _dt, Eigen::VectorXs& _delta)
 {
     _delta.head(3) = _data.head(3);
     new (&q_out_) Eigen::Map<Eigen::Quaternions>(_delta.data() + 3);
diff --git a/src/processor_preintegrated_imu.h b/src/processor_preintegrated_imu.h
index e9adb24240d65263c798ffa4c06a794d13b3679e..12bebb8126f4b5d8e16b7ea4bae531def4e54b98 100644
--- a/src/processor_preintegrated_imu.h
+++ b/src/processor_preintegrated_imu.h
@@ -70,11 +70,11 @@ class ProcessorPreintegratedIMU : public ProcessorMotion2{
     private:
         ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
         ///< (first-order propagation from *measurementCovariance*).
-        Eigen::Matrix<WolfScalar,9,9> preint_meas_cov_;
+        Eigen::Matrix<Scalar,9,9> preint_meas_cov_;
 
         ///Jacobians
-        Eigen::Matrix<WolfScalar,9,3> preintegrated_H_biasAcc_;
-        Eigen::Matrix<WolfScalar,9,3> preintegrated_H_biasOmega_;
+        Eigen::Matrix<Scalar,9,3> preintegrated_H_biasAcc_;
+        Eigen::Matrix<Scalar,9,3> preintegrated_H_biasOmega_;
 
 };
 
diff --git a/src/processor_tracker_feature.h b/src/processor_tracker_feature.h
index 7eabf6f3d46bb17c0ed47249051dde4264527dd4..dbda15cc0073ca0450a7546a79e67340cbbfe867 100644
--- a/src/processor_tracker_feature.h
+++ b/src/processor_tracker_feature.h
@@ -18,14 +18,14 @@ namespace wolf
 struct FeatureMatch
 {
         FeatureBase* feature_ptr_;
-        WolfScalar normalized_score_;
+        Scalar normalized_score_;
 
         FeatureMatch() :
                 feature_ptr_(nullptr), normalized_score_(0.0)
         {
 
         }
-        FeatureMatch(FeatureBase* _last_feature_ptr, const WolfScalar& _normalized_score) :
+        FeatureMatch(FeatureBase* _last_feature_ptr, const Scalar& _normalized_score) :
                 feature_ptr_(_last_feature_ptr), normalized_score_(_normalized_score)
         {
 
diff --git a/src/processor_tracker_landmark.h b/src/processor_tracker_landmark.h
index cb6673ff3c8ad8748d4512e0f82e13bd5285b5f9..00e7e2f47ee24f34dcf8481c9fd0d9f681492fa8 100644
--- a/src/processor_tracker_landmark.h
+++ b/src/processor_tracker_landmark.h
@@ -18,13 +18,13 @@ namespace wolf
 struct LandmarkMatch
 {
         LandmarkBase* landmark_ptr_;
-        WolfScalar normalized_score_;
+        Scalar normalized_score_;
 
         LandmarkMatch() :
                 landmark_ptr_(nullptr), normalized_score_(0.0)
         {
         }
-        LandmarkMatch(LandmarkBase* _landmark_ptr, const WolfScalar& _normalized_score) :
+        LandmarkMatch(LandmarkBase* _landmark_ptr, const Scalar& _normalized_score) :
                 landmark_ptr_(_landmark_ptr), normalized_score_(_normalized_score)
         {
 
diff --git a/src/processor_tracker_laser.cpp b/src/processor_tracker_laser.cpp
index 32bb14a1febcf767140661410ed632fc5b8569ce..36453e2f4e89a41b8d122660b900a39f14d8fdcf 100644
--- a/src/processor_tracker_laser.cpp
+++ b/src/processor_tracker_laser.cpp
@@ -38,7 +38,7 @@ unsigned int ProcessorTrackerLaser::findLandmarks(LandmarkBaseList& _landmark_li
     if (!new_features_incoming_.empty())
     {
         //local declarations
-        WolfScalar prob, dm2;
+        Scalar prob, dm2;
         unsigned int ii, jj;
         // COMPUTING ALL EXPECTED FEATURES
         std::map<LandmarkBase*, Eigen::Vector4s> expected_features;
@@ -140,11 +140,11 @@ void ProcessorTrackerLaser::extractCorners(const CaptureLaser2D* _capture_laser_
         measurement(2) = corner.orientation_;
         measurement(3) = corner.aperture_;
         // TODO: maybe in line object?
-        WolfScalar L1 = corner.line_1_.length();
-        WolfScalar L2 = corner.line_2_.length();
-        WolfScalar cov_angle_line1 = 12 * corner.line_1_.error_
+        Scalar L1 = corner.line_1_.length();
+        Scalar L2 = corner.line_2_.length();
+        Scalar cov_angle_line1 = 12 * corner.line_1_.error_
                 / (pow(L1, 2) * (pow(corner.line_1_.np_, 3) - pow(corner.line_1_.np_, 2)));
-        WolfScalar cov_angle_line2 = 12 * corner.line_2_.error_
+        Scalar cov_angle_line2 = 12 * corner.line_2_.error_
                 / (pow(L2, 2) * (pow(corner.line_2_.np_, 3) - pow(corner.line_2_.np_, 2)));
         //init cov in corner coordinates
         measurement_cov << corner.line_1_.error_ + cov_angle_line1 * L1 * L1 / 4, 0, 0, 0, 0, corner.line_2_.error_
@@ -190,7 +190,7 @@ void ProcessorTrackerLaser::expectedFeature(LandmarkBase* _landmark_ptr, Eigen::
     {
         // Jacobian
         Eigen::Vector2s p_robot_landmark = t_world_robot_.head(2) - _landmark_ptr->getPPtr()->getVector();
-        Eigen::Matrix<WolfScalar, 3, 6> Jlr = Eigen::Matrix<WolfScalar, 3, 6>::Zero();
+        Eigen::Matrix<Scalar, 3, 6> Jlr = Eigen::Matrix<Scalar, 3, 6>::Zero();
         Jlr.block<3, 3>(0, 3) = -R_world_sensor_.transpose();
         Jlr.block<3, 3>(0, 3) = R_world_sensor_.transpose();
         Jlr(0, 2) = -p_robot_landmark(0) * sin(t_world_sensor_(2)) + p_robot_landmark(1) * cos(t_world_sensor_(2));
@@ -210,7 +210,7 @@ Eigen::VectorXs ProcessorTrackerLaser::computeSquaredMahalanobisDistances(const
 {
 
     const Eigen::Vector2s& p_feature = _feature_ptr->getMeasurement().head(2);
-    const WolfScalar& o_feature = _feature_ptr->getMeasurement()(2);
+    const Scalar& o_feature = _feature_ptr->getMeasurement()(2);
     // ------------------------ d
     Eigen::Vector3s d;
     d.head(2) = p_feature - _expected_feature.head(2);
diff --git a/src/processor_tracker_laser.h b/src/processor_tracker_laser.h
index 3d21636b6d4d46ee4f5549018a2bf5d0d5792b81..5ef0cfb44c1a5cd6b183bb580ed96ec0458c6ff9 100644
--- a/src/processor_tracker_laser.h
+++ b/src/processor_tracker_laser.h
@@ -26,10 +26,10 @@ namespace wolf
 {
 
 //some consts.. TODO: this tuning params should be grouped in a struct and passed to the class from ros node, at constructor level
-const WolfScalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees
-const WolfScalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
-const WolfScalar position_error_th_ = 1;
-const WolfScalar min_features_ratio_th_ = 0.5;
+const Scalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees
+const Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
+const Scalar position_error_th_ = 1;
+const Scalar min_features_ratio_th_ = 0.5;
 
 class ProcessorTrackerLaser : public ProcessorTrackerLandmark
 {
diff --git a/src/sensor_gps_fix.cpp b/src/sensor_gps_fix.cpp
index 0b13f59c413e29fb0daf1b68dc43b8f855906ec4..364729513c3126d50187bd8d22272402fedd4af2 100644
--- a/src/sensor_gps_fix.cpp
+++ b/src/sensor_gps_fix.cpp
@@ -13,7 +13,7 @@ SensorGPSFix::~SensorGPSFix()
     //
 }
 
-WolfScalar SensorGPSFix::getNoise() const
+Scalar SensorGPSFix::getNoise() const
 {
     return noise_std_(0);
 }
diff --git a/src/sensor_laser_2D.cpp b/src/sensor_laser_2D.cpp
index 2e6f2aed4a90d682509fe13e7d3012b1654214f1..6f1e0472f64e8ee63f3f8e19c316c06f02ecda4e 100644
--- a/src/sensor_laser_2D.cpp
+++ b/src/sensor_laser_2D.cpp
@@ -2,9 +2,9 @@
 
 namespace wolf {
 
-// SensorLaser2D::SensorLaser2D(const Eigen::VectorXs & _sp, WolfScalar _angle_min, WolfScalar _angle_max, WolfScalar _angle_increment, WolfScalar _range_min, WolfScalar _range_max, WolfScalar _range_stdev, WolfScalar _time_increment, WolfScalar _scan_time) :
+// SensorLaser2D::SensorLaser2D(const Eigen::VectorXs & _sp, Scalar _angle_min, Scalar _angle_max, Scalar _angle_increment, Scalar _range_min, Scalar _range_max, Scalar _range_stdev, Scalar _time_increment, Scalar _scan_time) :
 //         SensorBase(LIDAR, _sp, 8)
-// //        SensorBase(LIDAR, _sp,{(WolfScalar)(_nrays), _apert, _rmin, _rmax, _range_stdev})
+// //        SensorBase(LIDAR, _sp,{(Scalar)(_nrays), _apert, _rmin, _rmax, _range_stdev})
 // {
 //     params_ << _angle_min, _angle_max, _angle_increment, _range_min, _range_max, _range_stdev, _time_increment, _scan_time;
 // }
@@ -89,42 +89,42 @@ const laserscanutils::ExtractCornerParams & SensorLaser2D::getCornerAlgParams()
     return corners_alg_params_;
 }
 
-/*WolfScalar SensorLaser2D::getAngleMin() const
+/*Scalar SensorLaser2D::getAngleMin() const
 {
     return params_(0);
 }
 
-WolfScalar SensorLaser2D::getAngleMax() const
+Scalar SensorLaser2D::getAngleMax() const
 {
     return params_(1);
 }
 
-WolfScalar SensorLaser2D::getAngleIncrement() const
+Scalar SensorLaser2D::getAngleIncrement() const
 {
     return params_(2);
 }
 
-WolfScalar SensorLaser2D::getRangeMin() const
+Scalar SensorLaser2D::getRangeMin() const
 {
     return params_(3);
 }
 
-WolfScalar SensorLaser2D::getRangeMax() const
+Scalar SensorLaser2D::getRangeMax() const
 {
     return params_(4);
 }
 
-WolfScalar SensorLaser2D::getRangeStdDev() const
+Scalar SensorLaser2D::getRangeStdDev() const
 {
     return params_(5);
 }
 
-WolfScalar SensorLaser2D::getTimeIncrement() const
+Scalar SensorLaser2D::getTimeIncrement() const
 {
     return params_(6);
 }
 
-WolfScalar SensorLaser2D::getScanTime() const
+Scalar SensorLaser2D::getScanTime() const
 {
     return params_(7);
 }*/
diff --git a/src/sensor_laser_2D.h b/src/sensor_laser_2D.h
index 75e9c1f6cffe34edc6971c8158678c18257a41a9..9b515ef2b68a676ed2cb81c110491c707262234e 100644
--- a/src/sensor_laser_2D.h
+++ b/src/sensor_laser_2D.h
@@ -74,56 +74,56 @@ class SensorLaser2D : public SensorBase
          * Returns angle_min
          *
          **/
-//         WolfScalar getAngleMin() const;
+//         Scalar getAngleMin() const;
 
         /** \brief Returns angle_max
          *
          * Returns angle_max
          *
          **/
-//         WolfScalar getAngleMax() const;
+//         Scalar getAngleMax() const;
 
         /** \brief Returns angle_increment
          *
          * Returns angle_increment
          *
          **/
-//         WolfScalar getAngleIncrement() const;
+//         Scalar getAngleIncrement() const;
 
         /** \brief Returns range_min
          *
          * Returns range_min
          *
          **/
-//         WolfScalar getRangeMin() const;
+//         Scalar getRangeMin() const;
 
         /** \brief Returns range_max
          *
          * Returns range_max
          *
          **/
-//         WolfScalar getRangeMax() const;
+//         Scalar getRangeMax() const;
         
         /** \brief Returns _range_stdev
          * 
          * Returns _range_stdev
          * 
          **/        
-//         WolfScalar getRangeStdDev() const;
+//         Scalar getRangeStdDev() const;
 
         /** \brief Returns time_increment
          *
          * Returns time_increment
          *
          **/
-//         WolfScalar getTimeIncrement() const;
+//         Scalar getTimeIncrement() const;
 
         /** \brief Returns scan_time
          *
          * Returns scan_time
          *
          **/
-//         WolfScalar getScanTime() const;
+//         Scalar getScanTime() const;
 
         /** \brief Prints parameters
          **/                
diff --git a/src/sensor_odom_2D.cpp b/src/sensor_odom_2D.cpp
index a6560c34ea7ccbd9b23278a2dc409e2773ee707a..9fefe16d7f4d1aa1dd89ed99d6af315e9a2fef99 100644
--- a/src/sensor_odom_2D.cpp
+++ b/src/sensor_odom_2D.cpp
@@ -2,7 +2,7 @@
 
 namespace wolf {
 
-SensorOdom2D::SensorOdom2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const WolfScalar& _disp_noise_factor, const WolfScalar&  _rot_noise_factor) :
+SensorOdom2D::SensorOdom2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const Scalar& _disp_noise_factor, const Scalar&  _rot_noise_factor) :
         SensorBase(SEN_ODOM_2D, _p_ptr, _o_ptr, nullptr, 2), k_disp_to_disp_(_disp_noise_factor), k_rot_to_rot_(_rot_noise_factor)
 {
 }
@@ -12,12 +12,12 @@ SensorOdom2D::~SensorOdom2D()
     //
 }
 
-WolfScalar SensorOdom2D::getDispVarToDispNoiseFactor() const
+Scalar SensorOdom2D::getDispVarToDispNoiseFactor() const
 {
     return k_disp_to_disp_;
 }
 
-WolfScalar SensorOdom2D::getRotVarToRotNoiseFactor() const
+Scalar SensorOdom2D::getRotVarToRotNoiseFactor() const
 {
     return k_rot_to_rot_;
 }
diff --git a/src/sensor_odom_2D.h b/src/sensor_odom_2D.h
index 9edb5150b4a4d65b968eb5c3436a1a0426f17806..2f1e1d47ef7c0909443c533f817f7d52f72e3be7 100644
--- a/src/sensor_odom_2D.h
+++ b/src/sensor_odom_2D.h
@@ -11,8 +11,8 @@ class SensorOdom2D : public SensorBase
 {
 
     protected:
-        WolfScalar k_disp_to_disp_; ///< ratio of displacement variance to displacement, for odometry noise calculation
-        WolfScalar k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation
+        Scalar k_disp_to_disp_; ///< ratio of displacement variance to displacement, for odometry noise calculation
+        Scalar k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation
 
 	public:
         /** \brief Constructor with arguments
@@ -24,7 +24,7 @@ class SensorOdom2D : public SensorBase
          * \param _rot_noise_factor rotation noise factor
          * 
          **/
-		SensorOdom2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const WolfScalar& _disp_noise_factor, const WolfScalar&  _rot_noise_factor);
+		SensorOdom2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const Scalar& _disp_noise_factor, const Scalar&  _rot_noise_factor);
 
         /** \brief Default destructor (not recommended)
          *
diff --git a/src/solver/cost_function_sparse_base.h b/src/solver/cost_function_sparse_base.h
index af0072e323e4920ff6982abf09a44ef18c55fe62..03b7b605380c015234758d8a9907206c7d50a9b5 100644
--- a/src/solver/cost_function_sparse_base.h
+++ b/src/solver/cost_function_sparse_base.h
@@ -32,7 +32,7 @@ template <class ConstraintT,
                 unsigned int BLOCK_9_SIZE = 0>
 class CostFunctionSparseBase : CostFunctionBase
 {
-        typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE +
+        typedef ceres::Jet<Scalar, BLOCK_0_SIZE +
                                        BLOCK_1_SIZE +
                                        BLOCK_2_SIZE +
                                        BLOCK_3_SIZE +
diff --git a/src/state_block.h b/src/state_block.h
index 730c5814cd7753e09c85cbf4726445617ff466ef..eabbe035b2c634773f10af10e8f00d4a649985ea 100644
--- a/src/state_block.h
+++ b/src/state_block.h
@@ -53,7 +53,7 @@ class StateBlock
 
         /** \brief Returns the pointer to the first element of the state
          **/
-        WolfScalar* getPtr();
+        Scalar* getPtr();
         
         /** \brief Returns the state vector
          **/
@@ -102,7 +102,7 @@ inline StateBlock::~StateBlock()
     //
 }
 
-inline WolfScalar* StateBlock::getPtr()
+inline Scalar* StateBlock::getPtr()
 {
     return state_.data();
 }
diff --git a/src/time_stamp.cpp b/src/time_stamp.cpp
index 82b57ef152452825cd1c25804b64b0216e793fec..3d4bbef869717f8d20a336011e59128bce2b50af 100644
--- a/src/time_stamp.cpp
+++ b/src/time_stamp.cpp
@@ -9,14 +9,14 @@ TimeStamp::TimeStamp() :
     setToNow();
 }
 
-TimeStamp::TimeStamp(const WolfScalar _ts) :
+TimeStamp::TimeStamp(const Scalar _ts) :
         time_stamp_(_ts)
 {
     //
 }
 
 TimeStamp::TimeStamp(const unsigned long int _sec, const unsigned long int _nsec) :
-        time_stamp_((WolfScalar)_sec + (WolfScalar)_nsec/(WolfScalar)1e9)
+        time_stamp_((Scalar)_sec + (Scalar)_nsec/(Scalar)1e9)
 {
     //
 }
diff --git a/src/time_stamp.h b/src/time_stamp.h
index ccadd40fbe7d2bc0e5c1ded187797e825718441a..9e937314552253235e41ddf1a35f9fa6c83750b5 100644
--- a/src/time_stamp.h
+++ b/src/time_stamp.h
@@ -22,7 +22,7 @@ namespace wolf {
 class TimeStamp
 {
     private:
-        WolfScalar time_stamp_; ///< Time stamp. Expressed in seconds from 1th jan 1970.
+        Scalar time_stamp_; ///< Time stamp. Expressed in seconds from 1th jan 1970.
         static const unsigned int TIME_STAMP_DIGITS_ = 10; ///< Number of digits to print time stamp values        
 
     public:
@@ -38,7 +38,7 @@ class TimeStamp
          * Constructor with arguments
          *
          */
-        TimeStamp(const WolfScalar _ts);
+        TimeStamp(const Scalar _ts);
 
         /** \brief Constructor from sec and nsec
          *
@@ -76,14 +76,14 @@ class TimeStamp
          * Sets time stamp to a given value passed as a scalar_t (seconds)
          *
          */
-        void set(const WolfScalar ts);
+        void set(const Scalar ts);
 
         /** \brief Get time stamp
          *
          * Returns time stamp
          *
          */
-        WolfScalar get() const;
+        Scalar get() const;
 
         /** \brief Get time stamp (only seconds)
          *
@@ -104,7 +104,7 @@ class TimeStamp
          * Assignement operator
          * 
          */
-        void operator =(const WolfScalar& ts);
+        void operator =(const Scalar& ts);
 
         /** \brief Assignement operator
          * 
@@ -132,15 +132,15 @@ class TimeStamp
          * difference operator
          * 
          */
-        WolfScalar operator -(const TimeStamp& ts) const;
+        Scalar operator -(const TimeStamp& ts) const;
 
         /** \brief Add-assign operator
          */
-        void operator +=(const WolfScalar& dt);
+        void operator +=(const Scalar& dt);
 
         /** \brief Add-assign operator
          */
-        TimeStamp operator +(const WolfScalar& dt);
+        TimeStamp operator +(const Scalar& dt);
 
         /** \brief Prints time stamp to a given ostream
          *
@@ -155,25 +155,25 @@ inline void TimeStamp::setToNow()
 {
     timeval ts;
     gettimeofday(&ts, NULL);
-    time_stamp_ = (WolfScalar)(ts.tv_sec) + (WolfScalar)(ts.tv_usec) / 1e6;
+    time_stamp_ = (Scalar)(ts.tv_sec) + (Scalar)(ts.tv_usec) / 1e6;
 }
 
-inline void TimeStamp::set(const WolfScalar ts)
+inline void TimeStamp::set(const Scalar ts)
 {
     time_stamp_ = ts;
 }
 
 inline void TimeStamp::set(const unsigned long int sec, const unsigned long int nanosec)
 {
-    time_stamp_ = (WolfScalar)(sec) + (WolfScalar)(nanosec) / (WolfScalar)(1e9);
+    time_stamp_ = (Scalar)(sec) + (Scalar)(nanosec) / (Scalar)(1e9);
 }
 
 inline void TimeStamp::set(const timeval& ts)
 {
-    time_stamp_ = (WolfScalar)(ts.tv_sec) + (WolfScalar)(ts.tv_usec) / 1e6;
+    time_stamp_ = (Scalar)(ts.tv_sec) + (Scalar)(ts.tv_usec) / 1e6;
 }
 
-inline WolfScalar TimeStamp::get() const
+inline Scalar TimeStamp::get() const
 {
     return time_stamp_;
 }
@@ -187,8 +187,8 @@ inline unsigned long int TimeStamp::getSeconds() const
 
 inline unsigned long int TimeStamp::getNanoSeconds() const
 {
-    WolfScalar ts;
-    ts = (WolfScalar)((floor(time_stamp_)));
+    Scalar ts;
+    ts = (Scalar)((floor(time_stamp_)));
     return (unsigned long int)((time_stamp_ - ts) * 1e9);
 }
 
@@ -197,7 +197,7 @@ inline void TimeStamp::operator =(const TimeStamp& ts)
     time_stamp_ = ts.get();
 }
 
-inline void TimeStamp::operator =(const WolfScalar& ts)
+inline void TimeStamp::operator =(const Scalar& ts)
 {
     time_stamp_ = ts;
 }
@@ -218,17 +218,17 @@ inline bool TimeStamp::operator <=(const TimeStamp& ts) const
         return false;
 }
 
-inline WolfScalar TimeStamp::operator -(const TimeStamp& ts) const
+inline Scalar TimeStamp::operator -(const TimeStamp& ts) const
 {
     return (time_stamp_ - ts.get());
 }
 
-inline void TimeStamp::operator +=(const WolfScalar& dt)
+inline void TimeStamp::operator +=(const Scalar& dt)
 {
     time_stamp_ += dt;
 }
 
-inline TimeStamp TimeStamp::operator +(const WolfScalar& dt)
+inline TimeStamp TimeStamp::operator +(const Scalar& dt)
 {
     return TimeStamp(time_stamp_ + dt);
 }
diff --git a/src/wolf.h b/src/wolf.h
index e0b381574040c925d4f321f065b4a08cdfc33121..da526dd586864dcf44d0a5606fa42e474fdd9597 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -33,9 +33,9 @@ namespace wolf {
  *
  * The ONLY exception to this rule is when you need special precision. The ONLY example by now is the time stamp which uses double.
  */
-//typedef float WolfScalar;         // Use this for float, 32 bit precision
-typedef double WolfScalar;        // Use this for double, 64 bit precision
-//typedef long double WolfScalar;   // Use this for long double, 128 bit precision
+//typedef float Scalar;         // Use this for float, 32 bit precision
+typedef double Scalar;        // Use this for double, 64 bit precision
+//typedef long double Scalar;   // Use this for long double, 128 bit precision
 
 namespace Constants{
 
@@ -52,7 +52,7 @@ const double MIN_VARIANCE = 1e-6;
 } // namespace wolf
 
 ///////////////////////////////////////////
-// Construct types for any scalar defined in the typedef WolfScalar above
+// Construct types for any scalar defined in the typedef Scalar above
 ////////////////////////////////////////////
 /** \brief Namespace extending Eigen definitions
  *
@@ -60,34 +60,34 @@ const double MIN_VARIANCE = 1e-6;
  * The appended letter indicating this is 's', so that we have, e.g.,
  * - VectorXf   Vector of floats - defined by Eigen
  * - VectorXd   Vector of doubles - defined by Eigen
- * - VectorXs   Vector of either double of float, depending on the type \b WolfScalar, defined by Wolf.
+ * - VectorXs   Vector of either double of float, depending on the type \b Scalar, defined by Wolf.
  * 
  */
 namespace Eigen  // Eigen namespace extension
 {
 // 1. Vectors and Matrices
-typedef Matrix<wolf::WolfScalar, 2, 2, Eigen::RowMajor> Matrix2s;                ///< 2x2 matrix of real WolfScalar type
-typedef Matrix<wolf::WolfScalar, 3, 3, Eigen::RowMajor> Matrix3s;                ///< 3x3 matrix of real WolfScalar type
-typedef Matrix<wolf::WolfScalar, 4, 4, Eigen::RowMajor> Matrix4s;                ///< 4x4 matrix of real WolfScalar type
-typedef Matrix<wolf::WolfScalar, 7, 7, Eigen::RowMajor> Matrix7s;                ///< 7x7 matrix of real WolfScalar type
-typedef Matrix<wolf::WolfScalar, Dynamic, Dynamic, Eigen::RowMajor> MatrixXs;    ///< variable size matrix of real WolfScalar type
-typedef Matrix<wolf::WolfScalar, 1, 1> Vector1s;                ///< 1-vector of real WolfScalar type
-typedef Matrix<wolf::WolfScalar, 2, 1> Vector2s;                ///< 2-vector of real WolfScalar type
-typedef Matrix<wolf::WolfScalar, 3, 1> Vector3s;                ///< 3-vector of real WolfScalar type
-typedef Matrix<wolf::WolfScalar, 4, 1> Vector4s;                ///< 4-vector of real WolfScalar type
-typedef Matrix<wolf::WolfScalar, 6, 1> Vector6s;                ///< 6-vector of real WolfScalar type
-typedef Matrix<wolf::WolfScalar, 7, 1> Vector7s;                ///< 7-vector of real WolfScalar type
-typedef Matrix<wolf::WolfScalar, Dynamic, 1> VectorXs;          ///< variable size vector of real WolfScalar type
-typedef Matrix<wolf::WolfScalar, 1, 2> RowVector2s;             ///< 2-row-vector of real WolfScalar type
-typedef Matrix<wolf::WolfScalar, 1, 3> RowVector3s;             ///< 3-row-vector of real WolfScalar type
-typedef Matrix<wolf::WolfScalar, 1, 4> RowVector4s;             ///< 4-row-vector of real WolfScalar type
-typedef Matrix<wolf::WolfScalar, 1, 7> RowVector7s;             ///< 7-row-vector of real WolfScalar type
-typedef Matrix<wolf::WolfScalar, 1, Dynamic> RowVectorXs;       ///< variable size row-vector of real WolfScalar type
+typedef Matrix<wolf::Scalar, 2, 2, Eigen::RowMajor> Matrix2s;                ///< 2x2 matrix of real Scalar type
+typedef Matrix<wolf::Scalar, 3, 3, Eigen::RowMajor> Matrix3s;                ///< 3x3 matrix of real Scalar type
+typedef Matrix<wolf::Scalar, 4, 4, Eigen::RowMajor> Matrix4s;                ///< 4x4 matrix of real Scalar type
+typedef Matrix<wolf::Scalar, 7, 7, Eigen::RowMajor> Matrix7s;                ///< 7x7 matrix of real Scalar type
+typedef Matrix<wolf::Scalar, Dynamic, Dynamic, Eigen::RowMajor> MatrixXs;    ///< variable size matrix of real Scalar type
+typedef Matrix<wolf::Scalar, 1, 1> Vector1s;                ///< 1-vector of real Scalar type
+typedef Matrix<wolf::Scalar, 2, 1> Vector2s;                ///< 2-vector of real Scalar type
+typedef Matrix<wolf::Scalar, 3, 1> Vector3s;                ///< 3-vector of real Scalar type
+typedef Matrix<wolf::Scalar, 4, 1> Vector4s;                ///< 4-vector of real Scalar type
+typedef Matrix<wolf::Scalar, 6, 1> Vector6s;                ///< 6-vector of real Scalar type
+typedef Matrix<wolf::Scalar, 7, 1> Vector7s;                ///< 7-vector of real Scalar type
+typedef Matrix<wolf::Scalar, Dynamic, 1> VectorXs;          ///< variable size vector of real Scalar type
+typedef Matrix<wolf::Scalar, 1, 2> RowVector2s;             ///< 2-row-vector of real Scalar type
+typedef Matrix<wolf::Scalar, 1, 3> RowVector3s;             ///< 3-row-vector of real Scalar type
+typedef Matrix<wolf::Scalar, 1, 4> RowVector4s;             ///< 4-row-vector of real Scalar type
+typedef Matrix<wolf::Scalar, 1, 7> RowVector7s;             ///< 7-row-vector of real Scalar type
+typedef Matrix<wolf::Scalar, 1, Dynamic> RowVectorXs;       ///< variable size row-vector of real Scalar type
 
 // 2. Quaternions and other rotation things
-typedef Quaternion<wolf::WolfScalar> Quaternions;               ///< Quaternion of real WolfScalar type
-typedef AngleAxis<wolf::WolfScalar> AngleAxiss;                 ///< Angle-Axis of real WolfScalar type
-typedef Rotation2D<wolf::WolfScalar> Rotation2Ds;               ///< Rotation2D of real WolfScalar type
+typedef Quaternion<wolf::Scalar> Quaternions;               ///< Quaternion of real Scalar type
+typedef AngleAxis<wolf::Scalar> AngleAxiss;                 ///< Angle-Axis of real Scalar type
+typedef Rotation2D<wolf::Scalar> Rotation2Ds;               ///< Rotation2D of real Scalar type
 }
 
 namespace wolf {
@@ -337,7 +337,7 @@ typedef std::list<StateBlock*> StateBlockList;
 typedef StateBlockList::iterator StateBlockIter;
 
 
-inline WolfScalar pi2pi(const WolfScalar& angle)
+inline Scalar pi2pi(const Scalar& angle)
 {
     return (angle > 0 ? fmod(angle + M_PI, 2 * M_PI) - M_PI : fmod(angle - M_PI, 2 * M_PI) + M_PI);
 }
@@ -347,7 +347,7 @@ inline WolfScalar pi2pi(const WolfScalar& angle)
 // Quaternion things
 namespace Eigen{
 inline void v2q(const Eigen::VectorXs& _v, Eigen::Quaternions& _q){
-    wolf::WolfScalar angle = _v.norm();
+    wolf::Scalar angle = _v.norm();
     if (angle < wolf::Constants::EPS)
         _q = Eigen::Quaternions::Identity();
     else
@@ -357,7 +357,7 @@ inline void v2q(const Eigen::VectorXs& _v, Eigen::Quaternions& _q){
 }
 
 inline void v2q(const Eigen::VectorXs& _v, Eigen::Map<Eigen::Quaternions>& _q){
-    wolf::WolfScalar angle = _v.norm();
+    wolf::Scalar angle = _v.norm();
     if (angle < wolf::Constants::EPS)
         _q = Eigen::Quaternions::Identity();
     else
diff --git a/src/wolf_manager.cpp b/src/wolf_manager.cpp
index 94d87b8d64004a889a15e877526b4378cb427964..620a33a2a3f39dff8b8c5da2fbefbf4667e7aaf6 100644
--- a/src/wolf_manager.cpp
+++ b/src/wolf_manager.cpp
@@ -8,7 +8,7 @@ WolfManager::WolfManager(const FrameStructure _frame_structure,
                          const Eigen::VectorXs& _prior,
                          const Eigen::MatrixXs& _prior_cov,
                          const unsigned int& _trajectory_size,
-                         const WolfScalar& _new_frame_elapsed_time) :
+                         const Scalar& _new_frame_elapsed_time) :
         problem_(new WolfProblem(_frame_structure)),
         sensor_prior_(_sensor_prior_ptr),
         current_frame_(nullptr),
@@ -230,7 +230,7 @@ void WolfManager::setWindowSize(const unsigned int& _size)
 }
 
 
-void WolfManager::setNewFrameElapsedTime(const WolfScalar& _dt)
+void WolfManager::setNewFrameElapsedTime(const Scalar& _dt)
 {
     new_frame_elapsed_time_ = _dt;
 }
diff --git a/src/wolf_manager.h b/src/wolf_manager.h
index 7b1c67030892e16446315da2bd05fed48ae09079..e3f852d51bc6da8289171df875d29a7988dc7e93 100644
--- a/src/wolf_manager.h
+++ b/src/wolf_manager.h
@@ -55,7 +55,7 @@ class WolfManager
 
         //Manager parameters
         unsigned int trajectory_size_;
-        WolfScalar new_frame_elapsed_time_;
+        Scalar new_frame_elapsed_time_;
 
     public:
         WolfManager(const FrameStructure _frame_structure,
@@ -63,7 +63,7 @@ class WolfManager
                     const Eigen::VectorXs& _prior,
                     const Eigen::MatrixXs& _prior_cov,
                     const unsigned int& _trajectory_size = 10,
-                    const WolfScalar& _new_frame_elapsed_time = 0.1);
+                    const Scalar& _new_frame_elapsed_time = 0.1);
 
         virtual ~WolfManager();
 
@@ -87,7 +87,7 @@ class WolfManager
 
         void setWindowSize(const unsigned int& _size);
 
-        void setNewFrameElapsedTime(const WolfScalar& _dt);
+        void setNewFrameElapsedTime(const Scalar& _dt);
 };
 
 } // namespace wolf
diff --git a/src/wolf_problem.h b/src/wolf_problem.h
index 0c360423028839a44ead3aeec37ebadac2917711..09e75b2fd7c83c0f9a5c8471838d662263654341 100644
--- a/src/wolf_problem.h
+++ b/src/wolf_problem.h
@@ -45,7 +45,7 @@ class WolfProblem : public NodeBase
         StateBlockList state_block_ptr_list_;
         std::list<StateBlock*> state_block_add_list_;
         std::list<StateBlock*> state_block_update_list_;
-        std::list<WolfScalar*> state_block_remove_list_;
+        std::list<Scalar*> state_block_remove_list_;
         std::list<ConstraintBase*> constraint_add_list_;
         std::list<unsigned int> constraint_remove_list_;
 
@@ -197,7 +197,7 @@ class WolfProblem : public NodeBase
 
         /** \brief Gets a queue of state blocks to be removed from the solver
          */
-        std::list<WolfScalar*>* getStateBlockRemoveList();
+        std::list<Scalar*>* getStateBlockRemoveList();
 
         /** \brief Gets a queue of constraint ids to be added in the solver
          */
@@ -230,7 +230,7 @@ class WolfProblem : public NodeBase
 namespace wolf
 {
 
-inline std::list<WolfScalar*>* WolfProblem::getStateBlockRemoveList()
+inline std::list<Scalar*>* WolfProblem::getStateBlockRemoveList()
 {
     return &state_block_remove_list_;
 }