diff --git a/CMakeLists.txt b/CMakeLists.txt
index 67ef6f0b03b6d7d9c8638a38439607ddb35b09d9..4c6e49d2fe919dabdaa1ff01f1ec18589edb8e52 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,5 @@
 # Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+CMAKE_MINimuM_REQUIRED(VERSION 2.6)
 
 if(COMMAND cmake_policy)
   cmake_policy(SET CMP0005 NEW) 
@@ -10,7 +10,7 @@ SET(CMAKE_MACOSX_RPATH 1)
 
 
 # The project name
-PROJECT(IMU)
+PROJECT(imu)
 set(PLUGIN_NAME "wolf${PROJECT_NAME}")
 
 
@@ -72,7 +72,7 @@ if(BUILD_TESTS)
 endif()
 
 MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
+CMAKE_MINimuM_REQUIRED(VERSION 2.8)
 
 #CMAKE modules
 
@@ -127,26 +127,26 @@ INCLUDE_DIRECTORIES(BEFORE "include")
 SET(HDRS_COMMON
   )
 SET(HDRS_MATH
-  include/IMU/math/IMU_tools.h
+  include/imu/math/imu_tools.h
   )
 SET(HDRS_UTILS
   )
 SET(HDRS_CAPTURE
-  include/IMU/capture/capture_IMU.h
+  include/imu/capture/capture_imu.h
   )
 SET(HDRS_FACTOR
-  include/IMU/factor/factor_IMU.h
+  include/imu/factor/factor_imu.h
   )
 SET(HDRS_FEATURE
-  include/IMU/feature/feature_IMU.h
+  include/imu/feature/feature_imu.h
   )
 SET(HDRS_LANDMARK
   )
 SET(HDRS_PROCESSOR
-  include/IMU/processor/processor_IMU.h
+  include/imu/processor/processor_imu.h
   )
 SET(HDRS_SENSOR
-  include/IMU/sensor/sensor_IMU.h
+  include/imu/sensor/sensor_imu.h
   )
 SET(HDRS_SOLVER
   )
@@ -158,35 +158,35 @@ SET(HDRS_DTASSC
 SET(SRCS_COMMON
   )
 SET(SRCS_MATH
-  include/IMU/math/IMU_tools.h
+  include/imu/math/imu_tools.h
   )
 SET(SRCS_UTILS
   )
 
 SET(SRCS_CAPTURE
-  src/capture/capture_IMU.cpp
+  src/capture/capture_imu.cpp
   )
 SET(SRCS_FACTOR
   )
 SET(SRCS_FEATURE
-  src/feature/feature_IMU.cpp
+  src/feature/feature_imu.cpp
   )
 SET(SRCS_LANDMARK
   )
 SET(SRCS_PROCESSOR
-  src/processor/processor_IMU.cpp
-  test/processor_IMU_UnitTester.cpp
+  src/processor/processor_imu.cpp
+  test/processor_imu_UnitTester.cpp
 )
 SET(SRCS_SENSOR
-  src/sensor/sensor_IMU.cpp
+  src/sensor/sensor_imu.cpp
   )
 SET(SRCS_DTASSC
   )
 SET(SRCS_SOLVER
   )
 SET(SRCS_YAML
-  src/yaml/processor_IMU_yaml.cpp
-  src/yaml/sensor_IMU_yaml.cpp
+  src/yaml/processor_imu_yaml.cpp
+  src/yaml/sensor_imu_yaml.cpp
   )
 #OPTIONALS
 #optional HDRS and SRCS
@@ -287,7 +287,7 @@ INSTALL(FILES ${HDRS_YAML}
 
 INSTALL(FILES ${PROJECT_NAME}.found
   DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME})
-FILE(WRITE IMU.found "")
+FILE(WRITE imu.found "")
 
 INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
   DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/internal)
diff --git a/cmake_modules/wolfIMUConfig.cmake b/cmake_modules/wolfIMUConfig.cmake
deleted file mode 100644
index fbf1da76c0e046f7f925509176a6ef17fbdc8995..0000000000000000000000000000000000000000
--- a/cmake_modules/wolfIMUConfig.cmake
+++ /dev/null
@@ -1,79 +0,0 @@
-#edit the following line to add the librarie's header files
-FIND_PATH(
-    wolfIMU_INCLUDE_DIRS
-    NAMES IMU.found
-    PATHS /usr/local/include/iri-algorithms/wolf/plugin_IMU)
-IF(wolfIMU_INCLUDE_DIRS)
-  MESSAGE("Found wolf IMU include dirs: ${wolfIMU_INCLUDE_DIRS}")
-ELSE(wolfIMU_INCLUDE_DIRS)
-  MESSAGE("Couldn't find wolf IMU include dirs")
-ENDIF(wolfIMU_INCLUDE_DIRS)
-
-FIND_LIBRARY(
-    wolfIMU_LIBRARIES
-    NAMES libwolfIMU.so libwolfIMU.dylib
-    PATHS /usr/local/lib/iri-algorithms)
-IF(wolfIMU_LIBRARIES)
-  MESSAGE("Found wolf IMU lib: ${wolfIMU_LIBRARIES}")
-ELSE(wolfIMU_LIBRARIES)
-  MESSAGE("Couldn't find wolf IMU lib")
-ENDIF(wolfIMU_LIBRARIES)
-
-IF (wolfIMU_INCLUDE_DIRS AND wolfIMU_LIBRARIES)
-   SET(wolfIMU_FOUND TRUE)
- ELSE(wolfIMU_INCLUDE_DIRS AND wolfIMU_LIBRARIES)
-   set(wolfIMU_FOUND FALSE)
-ENDIF (wolfIMU_INCLUDE_DIRS AND wolfIMU_LIBRARIES)
-
-IF (wolfIMU_FOUND)
-   IF (NOT wolfIMU_FIND_QUIETLY)
-      MESSAGE(STATUS "Found wolf IMU: ${wolfIMU_LIBRARIES}")
-   ENDIF (NOT wolfIMU_FIND_QUIETLY)
-ELSE (wolfIMU_FOUND)
-   IF (wolfIMU_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find wolf IMU")
-   ENDIF (wolfIMU_FIND_REQUIRED)
-ENDIF (wolfIMU_FOUND)
-
-
-macro(wolf_report_not_found REASON_MSG)
-  set(wolfIMU_FOUND FALSE)
-  unset(wolfIMU_INCLUDE_DIRS)
-  unset(wolfIMU_LIBRARIES)
-
-  # Reset the CMake module path to its state when this script was called.
-  set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
-
-  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
-  # FindPackage() use the camelcase library name, not uppercase.
-  if (wolfIMU_FIND_QUIETLY)
-    message(STATUS "Failed to find wolf IMU- " ${REASON_MSG} ${ARGN})
-  else (wolfIMU_FIND_REQUIRED)
-    message(FATAL_ERROR "Failed to find wolf IMU - " ${REASON_MSG} ${ARGN})
-  else()
-    # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
-    # that prevents generation, but continues configuration.
-    message(SEND_ERROR "Failed to find wolf IMU - " ${REASON_MSG} ${ARGN})
-  endif ()
-  return()
-endmacro(wolf_report_not_found)
-
-if(NOT wolfIMU_FOUND)
-  wolf_report_not_found("Something went wrong while setting up wolf IMU.")
-endif(NOT wolfIMU_FOUND)
-# Set the include directories for wolf (itself).
-set(wolfIMU_FOUND TRUE)
-
-# Now we gather all the required dependencies for Wolf IMU
-if(NOT wolf_FOUND)
-  FIND_PACKAGE(wolf REQUIRED)
-
-  #We reverse in order to insert at the start
-  list(REVERSE wolfIMU_INCLUDE_DIRS)
-  list(APPEND wolfIMU_INCLUDE_DIRS ${wolf_INCLUDE_DIRS})
-  list(REVERSE wolfIMU_INCLUDE_DIRS)
-
-  list(REVERSE wolfIMU_LIBRARIES)
-  list(APPEND wolfIMU_LIBRARIES ${wolf_LIBRARIES})
-  list(REVERSE wolfIMU_LIBRARIES)
-endif()
diff --git a/cmake_modules/wolfimuConfig.cmake b/cmake_modules/wolfimuConfig.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..a671544bc37e09358bb5d859d9d2c4d636a455bd
--- /dev/null
+++ b/cmake_modules/wolfimuConfig.cmake
@@ -0,0 +1,79 @@
+#edit the following line to add the librarie's header files
+FIND_PATH(
+    wolfimu_INCLUDE_DIRS
+    NAMES imu.found
+    PATHS /usr/local/include/iri-algorithms/wolf/plugin_imu)
+IF(wolfimu_INCLUDE_DIRS)
+  MESSAGE("Found wolf imu include dirs: ${wolfimu_INCLUDE_DIRS}")
+ELSE(wolfimu_INCLUDE_DIRS)
+  MESSAGE("Couldn't find wolf imu include dirs")
+ENDIF(wolfimu_INCLUDE_DIRS)
+
+FIND_LIBRARY(
+    wolfimu_LIBRARIES
+    NAMES libwolfimu.so libwolfimu.dylib
+    PATHS /usr/local/lib/iri-algorithms)
+IF(wolfimu_LIBRARIES)
+  MESSAGE("Found wolf imu lib: ${wolfimu_LIBRARIES}")
+ELSE(wolfimu_LIBRARIES)
+  MESSAGE("Couldn't find wolf imu lib")
+ENDIF(wolfimu_LIBRARIES)
+
+IF (wolfimu_INCLUDE_DIRS AND wolfimu_LIBRARIES)
+   SET(wolfimu_FOUND TRUE)
+ ELSE(wolfimu_INCLUDE_DIRS AND wolfimu_LIBRARIES)
+   set(wolfimu_FOUND FALSE)
+ENDIF (wolfimu_INCLUDE_DIRS AND wolfimu_LIBRARIES)
+
+IF (wolfimu_FOUND)
+   IF (NOT wolfimu_FIND_QUIETLY)
+      MESSAGE(STATUS "Found wolf imu: ${wolfimu_LIBRARIES}")
+   ENDIF (NOT wolfimu_FIND_QUIETLY)
+ELSE (wolfimu_FOUND)
+   IF (wolfimu_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find wolf imu")
+   ENDIF (wolfimu_FIND_REQUIRED)
+ENDIF (wolfimu_FOUND)
+
+
+macro(wolf_report_not_found REASON_MSG)
+  set(wolfimu_FOUND FALSE)
+  unset(wolfimu_INCLUDE_DIRS)
+  unset(wolfimu_LIBRARIES)
+
+  # Reset the CMake module path to its state when this script was called.
+  set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
+
+  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
+  # FindPackage() use the camelcase library name, not uppercase.
+  if (wolfimu_FIND_QUIETLY)
+    message(STATUS "Failed to find wolf imu- " ${REASON_MSG} ${ARGN})
+  else (wolfimu_FIND_REQUIRED)
+    message(FATAL_ERROR "Failed to find wolf imu - " ${REASON_MSG} ${ARGN})
+  else()
+    # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
+    # that prevents generation, but continues configuration.
+    message(SEND_ERROR "Failed to find wolf imu - " ${REASON_MSG} ${ARGN})
+  endif ()
+  return()
+endmacro(wolf_report_not_found)
+
+if(NOT wolfimu_FOUND)
+  wolf_report_not_found("Something went wrong while setting up wolf imu.")
+endif(NOT wolfimu_FOUND)
+# Set the include directories for wolf (itself).
+set(wolfimu_FOUND TRUE)
+
+# Now we gather all the required dependencies for Wolf imu
+if(NOT wolf_FOUND)
+  FIND_PACKAGE(wolf REQUIRED)
+
+  #We reverse in order to insert at the start
+  list(REVERSE wolfimu_INCLUDE_DIRS)
+  list(APPEND wolfimu_INCLUDE_DIRS ${wolf_INCLUDE_DIRS})
+  list(REVERSE wolfimu_INCLUDE_DIRS)
+
+  list(REVERSE wolfimu_LIBRARIES)
+  list(APPEND wolfimu_LIBRARIES ${wolf_LIBRARIES})
+  list(REVERSE wolfimu_LIBRARIES)
+endif()
diff --git a/demos/demo_factor_imu.cpp b/demos/demo_factor_imu.cpp
index c5488ed1f739e5a955dfbc6949437261e1c57e67..a9389857f8295696c5bda07c006d59ec06c4608f 100644
--- a/demos/demo_factor_imu.cpp
+++ b/demos/demo_factor_imu.cpp
@@ -1,7 +1,7 @@
 //Wolf
-#include "core/capture/capture_IMU.h"
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
+#include "core/capture/capture_imu.h"
+#include "core/processor/processor_imu.h"
+#include "core/sensor/sensor_imu.h"
 #include "core/capture/capture_pose.h"
 #include "core/common/wolf.h"
 #include "core/problem/problem.h"
diff --git a/demos/demo_imuDock.cpp b/demos/demo_imuDock.cpp
index 6c3d190af92ff9606d20ee1bb2cf7a879b2b16fd..73f693e491acf81eb4020cfc99099f66f09ed94e 100644
--- a/demos/demo_imuDock.cpp
+++ b/demos/demo_imuDock.cpp
@@ -5,8 +5,8 @@
  *      \author: Dinesh Atchuthan
  */
 
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
+#include "core/processor/processor_imu.h"
+#include "core/sensor/sensor_imu.h"
 #include "core/common/wolf.h"
 #include "core/problem/problem.h"
 #include "core/ceres_wrapper/ceres_manager.h"
diff --git a/demos/demo_imuDock_autoKFs.cpp b/demos/demo_imuDock_autoKFs.cpp
index c22345556e017078596b7f813be05fa9474967d4..cfd482b6faea95d6bd606c04fdc1e9370ba9a837 100644
--- a/demos/demo_imuDock_autoKFs.cpp
+++ b/demos/demo_imuDock_autoKFs.cpp
@@ -5,8 +5,8 @@
  *      \author: Dinesh Atchuthan
  */
 
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
+#include "core/processor/processor_imu.h"
+#include "core/sensor/sensor_imu.h"
 #include "core/common/wolf.h"
 #include "core/problem/problem.h"
 #include "core/ceres_wrapper/ceres_manager.h"
diff --git a/demos/demo_imuPlateform_Offline.cpp b/demos/demo_imuPlateform_Offline.cpp
index 019e921f81d1cbde012aae7187fc65ad3e0cdffe..75d7120d62200ce742249d3771288352dd679979 100644
--- a/demos/demo_imuPlateform_Offline.cpp
+++ b/demos/demo_imuPlateform_Offline.cpp
@@ -1,7 +1,7 @@
 //Wolf
-#include "core/capture/capture_IMU.h"
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
+#include "core/capture/capture_imu.h"
+#include "core/processor/processor_imu.h"
+#include "core/sensor/sensor_imu.h"
 #include "core/common/wolf.h"
 #include "core/problem/problem.h"
 #include "core/sensor/sensor_odom_3D.h"
diff --git a/demos/demo_imu_constrained0.cpp b/demos/demo_imu_constrained0.cpp
index 242fb18ac80b8c6c97b9d9180c57e47dc29b3f64..b6b2efe312a69a96a2e2964827bcd58938a717fd 100644
--- a/demos/demo_imu_constrained0.cpp
+++ b/demos/demo_imu_constrained0.cpp
@@ -1,7 +1,7 @@
 //Wolf
-#include "core/capture/capture_IMU.h"
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
+#include "core/capture/capture_imu.h"
+#include "core/processor/processor_imu.h"
+#include "core/sensor/sensor_imu.h"
 #include "core/common/wolf.h"
 #include "core/problem/problem.h"
 #include "core/sensor/sensor_odom_3D.h"
diff --git a/demos/demo_processor_imu.cpp b/demos/demo_processor_imu.cpp
index 93bfa93dbb6ddc6e53b52fae5bc0fcf372bb94cf..0cf32a889eaab51944831bdc0dd639cea1b3abeb 100644
--- a/demos/demo_processor_imu.cpp
+++ b/demos/demo_processor_imu.cpp
@@ -6,9 +6,9 @@
  */
 
 //Wolf
-#include "core/capture/capture_IMU.h"
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
+#include "core/capture/capture_imu.h"
+#include "core/processor/processor_imu.h"
+#include "core/sensor/sensor_imu.h"
 #include "core/common/wolf.h"
 #include "core/problem/problem.h"
 #include "core/state_block/state_block.h"
diff --git a/demos/demo_processor_imu_jacobians.cpp b/demos/demo_processor_imu_jacobians.cpp
index 0029a8bdddc020c9ef109cb1c57c6fae508384ba..6c0714c9c9968db72b580c2004f7a1810925e4c0 100644
--- a/demos/demo_processor_imu_jacobians.cpp
+++ b/demos/demo_processor_imu_jacobians.cpp
@@ -6,9 +6,9 @@
  */
 
 //Wolf
-#include "core/capture/capture_IMU.h"
-#include "core/sensor/sensor_IMU.h"
-#include <test/processor_IMU_UnitTester.h>
+#include "core/capture/capture_imu.h"
+#include "core/sensor/sensor_imu.h"
+#include <test/processor_imu_UnitTester.h>
 #include "core/common/wolf.h"
 #include "core/problem/problem.h"
 #include "core/state_block/state_block.h"
diff --git a/demos/processor_imu.yaml b/demos/processor_imu.yaml
index 5c623b82d4879cf3d9888870ef05083e050fdb91..3eba9dc3e7e6c69841f4cc0cbac9674028c1fe44 100644
--- a/demos/processor_imu.yaml
+++ b/demos/processor_imu.yaml
@@ -1,4 +1,4 @@
-type: "ProcessorIMU"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "ProcessorImu"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
 
 time_tolerance: 0.0025         # Time tolerance for joining KFs
 unmeasured_perturbation_std: 0.00001
diff --git a/demos/processor_odom_3D.yaml b/demos/processor_odom_3d.yaml
similarity index 86%
rename from demos/processor_odom_3D.yaml
rename to demos/processor_odom_3d.yaml
index c0dc6463ade663803424f87e1cc1898c61b17561..491e9c8d2db5f258e96d60d0a2ce8321834e9fa0 100644
--- a/demos/processor_odom_3D.yaml
+++ b/demos/processor_odom_3d.yaml
@@ -1,4 +1,4 @@
-type: "ProcessorOdom3D"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "ProcessorOdom3d"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
 
 time_tolerance:         0.01  # seconds
 
diff --git a/demos/sensor_imu.yaml b/demos/sensor_imu.yaml
index 941b84610845f31cfed813394d85c923d2a3211b..3c78a00d35c785fe73381d8f6ce99a705d27ce77 100644
--- a/demos/sensor_imu.yaml
+++ b/demos/sensor_imu.yaml
@@ -1,4 +1,4 @@
-type: "SensorIMU"             # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "SensorImu"             # This must match the KEY used in the SensorFactory. Otherwise it is an error.
 
 motion_variances: 
     a_noise:                0.053     # standard deviation of Acceleration noise (same for all the axis) in m/s2
diff --git a/demos/sensor_odom_3D.yaml b/demos/sensor_odom_3d.yaml
similarity index 77%
rename from demos/sensor_odom_3D.yaml
rename to demos/sensor_odom_3d.yaml
index 9fb43d4c30c0f5c01757362f6122d0cba98b14c5..58db1c088fbc80339a78ba815fddbaf69674d3b6 100644
--- a/demos/sensor_odom_3D.yaml
+++ b/demos/sensor_odom_3d.yaml
@@ -1,4 +1,4 @@
-type: "SensorOdom3D"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "SensorOdom3d"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
 
 k_disp_to_disp:   0.02  # m^2   / m
 k_disp_to_rot:    0.02  # rad^2 / m
diff --git a/include/IMU/sensor/sensor_IMU.h b/include/IMU/sensor/sensor_IMU.h
deleted file mode 100644
index c8e13a2d8bf608c61aef462ea5f81e58946b2a9f..0000000000000000000000000000000000000000
--- a/include/IMU/sensor/sensor_IMU.h
+++ /dev/null
@@ -1,121 +0,0 @@
-#ifndef SENSOR_IMU_H
-#define SENSOR_IMU_H
-
-//wolf includes
-#include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.hpp"
-#include <iostream>
-
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorIMU);
-
-
-struct ParamsSensorIMU : public ParamsSensorBase
-{
-        //noise std dev
-        double w_noise = 0.001; //standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
-        double a_noise = 0.04; //standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s)
-
-        //Initial biases std dev
-        double ab_initial_stdev = 0.01; //accelerometer micro_g/sec
-        double wb_initial_stdev = 0.01; //gyroscope rad/sec
-
-        // bias rate of change std dev
-        double ab_rate_stdev = 0.00001;
-        double wb_rate_stdev = 0.00001;
-
-        virtual ~ParamsSensorIMU() = default;
-    ParamsSensorIMU()
-    {
-        //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
-    }
-    ParamsSensorIMU(std::string _unique_name, const ParamsServer& _server):
-        ParamsSensorBase(_unique_name, _server)
-    {
-        w_noise             = _server.getParam<double>(_unique_name + "/w_noise");
-        a_noise             = _server.getParam<double>(_unique_name + "/a_noise");
-        ab_initial_stdev    = _server.getParam<double>(_unique_name + "/ab_initial_stdev");
-        wb_initial_stdev    = _server.getParam<double>(_unique_name + "/wb_initial_stdev");
-        ab_rate_stdev       = _server.getParam<double>(_unique_name + "/ab_rate_stdev");
-        wb_rate_stdev       = _server.getParam<double>(_unique_name + "/wb_rate_stdev");
-    }
-    std::string print() const
-    {
-        return "\n" + ParamsSensorBase::print()                           + "\n"
-            + "w_noise: "           + std::to_string(w_noise)           + "\n"
-            + "a_noise: "           + std::to_string(a_noise)           + "\n"
-            + "ab_initial_stdev: "  + std::to_string(ab_initial_stdev)  + "\n"
-            + "wb_initial_stdev: "  + std::to_string(wb_initial_stdev)  + "\n"
-            + "ab_rate_stdev: "     + std::to_string(ab_rate_stdev)     + "\n"
-            + "wb_rate_stdev: "     + std::to_string(wb_rate_stdev)     + "\n";
-    }
-};
-
-WOLF_PTR_TYPEDEFS(SensorIMU);
-
-class SensorIMU : public SensorBase
-{
-
-    protected:
-        double a_noise; //Power Spectral Density (same for all the axis) in micro_g/ sqrt(Hz)
-        double w_noise; //Rate Noise Spectral Density (same for all the axis) in deg/sec/ sqrt(Hz)
-
-        //This is a trial to factor how much can the bias change in 1 sec at most
-        double ab_initial_stdev; //accelerometer m/sec^s
-        double wb_initial_stdev; //gyroscope rad/sec
-        double ab_rate_stdev;    //accelerometer m/sec^2 / sqrt(sec)
-        double wb_rate_stdev;    //gyroscope rad/sec / sqrt(sec)
-
-    public:
-
-        SensorIMU(const Eigen::VectorXd& _extrinsics, const ParamsSensorIMU& _params);
-        SensorIMU(const Eigen::VectorXd& _extrinsics, ParamsSensorIMUPtr _params);
-
-        double getGyroNoise() const;
-        double getAccelNoise() const;
-        double getWbInitialStdev() const;
-        double getAbInitialStdev() const;
-        double getWbRateStdev() const;
-        double getAbRateStdev() const;
-
-        virtual ~SensorIMU();
-
-    public:
-        static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorBasePtr _intrinsics = nullptr);
-
-};
-
-inline double SensorIMU::getGyroNoise() const
-{
-    return w_noise;
-}
-
-inline double SensorIMU::getAccelNoise() const
-{
-    return a_noise;
-}
-
-inline double SensorIMU::getWbInitialStdev() const
-{
-    return wb_initial_stdev;
-}
-
-inline double SensorIMU::getAbInitialStdev() const
-{
-    return ab_initial_stdev;
-}
-
-inline double SensorIMU::getWbRateStdev() const
-{
-    return wb_rate_stdev;
-}
-
-inline double SensorIMU::getAbRateStdev() const
-{
-    return ab_rate_stdev;
-}
-
-} // namespace wolf
-
-#endif // SENSOR_IMU_H
diff --git a/include/IMU/capture/capture_IMU.h b/include/imu/capture/capture_imu.h
similarity index 72%
rename from include/IMU/capture/capture_IMU.h
rename to include/imu/capture/capture_imu.h
index c5c3174f954668ec3aa3ac29e89715f20cc8a7c4..d080a04f2e512b6ed40fe76737e3eb2df0bc718c 100644
--- a/include/IMU/capture/capture_IMU.h
+++ b/include/imu/capture/capture_imu.h
@@ -2,41 +2,41 @@
 #define CAPTURE_IMU_H
 
 //Wolf includes
-#include "IMU/math/IMU_tools.h"
+#include "imu/math/imu_tools.h"
 #include "core/capture/capture_motion.h"
 
 namespace wolf {
     
-WOLF_PTR_TYPEDEFS(CaptureIMU);
+WOLF_PTR_TYPEDEFS(CaptureImu);
 
-class CaptureIMU : public CaptureMotion
+class CaptureImu : public CaptureMotion
 {
     public:
 
-        CaptureIMU(const TimeStamp& _init_ts,
+        CaptureImu(const TimeStamp& _init_ts,
                    SensorBasePtr _sensor_ptr,
                    const Eigen::Vector6d& _data,
                    const Eigen::MatrixXd& _data_cov,
                    CaptureBasePtr _capture_origin_ptr = nullptr);
 
-        CaptureIMU(const TimeStamp& _init_ts,
+        CaptureImu(const TimeStamp& _init_ts,
                    SensorBasePtr _sensor_ptr,
                    const Eigen::Vector6d& _data,
                    const Eigen::MatrixXd& _data_cov,
                    const Vector6d& _bias,
                    CaptureBasePtr _capture_origin_ptr = nullptr);
 
-        virtual ~CaptureIMU();
+        virtual ~CaptureImu();
 
         virtual VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const override;
 
 };
 
-inline Eigen::VectorXd CaptureIMU::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
+inline Eigen::VectorXd CaptureImu::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
 {
     return imu::plus(_delta, _delta_error);
 }
 
 } // namespace wolf
 
-#endif // CAPTURE_IMU_H
+#endif // CAPTURE_Imu_H
diff --git a/include/IMU/factor/factor_fix_bias.h b/include/imu/factor/factor_fix_bias.h
similarity index 94%
rename from include/IMU/factor/factor_fix_bias.h
rename to include/imu/factor/factor_fix_bias.h
index 13150f83dab99d269a534c450d623a02981442fa..85a30d524d57efeda7f5704d4f708e074ac27db1 100644
--- a/include/IMU/factor/factor_fix_bias.h
+++ b/include/imu/factor/factor_fix_bias.h
@@ -3,8 +3,8 @@
 #define FACTOR_FIX_BIAS_H_
 
 //Wolf includes
-#include "IMU/capture/capture_IMU.h"
-#include "IMU/feature/feature_IMU.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/feature/feature_imu.h"
 #include "core/factor/factor_autodiff.h"
 #include "core/frame/frame_base.h"
 #include "core/math/rotations.h"
@@ -21,8 +21,8 @@ class FactorFixBias: public FactorAutodiff<FactorFixBias,6,3,3>
     public:
         FactorFixBias(FeatureBasePtr _ftr_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
                 FactorAutodiff<FactorFixBias, 6, 3, 3>("FactorFixBias",
-                        nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getAccBias(),
-                                          std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getGyroBias())
+                        nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, std::static_pointer_cast<CaptureImu>(_ftr_ptr->getCapture())->getAccBias(),
+                                          std::static_pointer_cast<CaptureImu>(_ftr_ptr->getCapture())->getGyroBias())
         {
             // std::cout << "created FactorFixBias " << std::endl;
         }
diff --git a/include/IMU/factor/factor_IMU.h b/include/imu/factor/factor_imu.h
similarity index 86%
rename from include/IMU/factor/factor_IMU.h
rename to include/imu/factor/factor_imu.h
index 60c94a9288d45d337d61280dc8d4d007cfa53101..bb81e4a653027feb75efd9eefd9ac189cbcf5c08 100644
--- a/include/IMU/factor/factor_IMU.h
+++ b/include/imu/factor/factor_imu.h
@@ -2,8 +2,8 @@
 #define FACTOR_IMU_THETA_H_
 
 //Wolf includes
-#include "IMU/feature/feature_IMU.h"
-#include "IMU/sensor/sensor_IMU.h"
+#include "imu/feature/feature_imu.h"
+#include "imu/sensor/sensor_imu.h"
 #include "core/factor/factor_autodiff.h"
 #include "core/math/rotations.h"
 
@@ -11,19 +11,19 @@
 
 namespace wolf {
     
-WOLF_PTR_TYPEDEFS(FactorIMU);
+WOLF_PTR_TYPEDEFS(FactorImu);
 
 //class
-class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>
+class FactorImu : public FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>
 {
     public:
-        FactorIMU(const FeatureIMUPtr& _ftr_ptr,
-                      const CaptureIMUPtr& _capture_origin_ptr,
+        FactorImu(const FeatureImuPtr& _ftr_ptr,
+                      const CaptureImuPtr& _capture_origin_ptr,
                       const ProcessorBasePtr& _processor_ptr,
                       bool _apply_loss_function,
                       FactorStatus _status = FAC_ACTIVE);
 
-        virtual ~FactorIMU() = default;
+        virtual ~FactorImu() = default;
 
         virtual std::string getTopology() const override
         {
@@ -153,13 +153,13 @@ class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>
 
 ///////////////////// IMPLEMENTAITON ////////////////////////////
 
-inline FactorIMU::FactorIMU(const FeatureIMUPtr&    _ftr_ptr,
-                            const CaptureIMUPtr&    _cap_origin_ptr,
+inline FactorImu::FactorImu(const FeatureImuPtr&    _ftr_ptr,
+                            const CaptureImuPtr&    _cap_origin_ptr,
                             const ProcessorBasePtr& _processor_ptr,
                             bool                    _apply_loss_function,
                             FactorStatus        _status) :
-                FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>( // ...
-                        "FactorIMU",
+                FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>( // ...
+                        "FactorImu",
                         _cap_origin_ptr->getFrame(),
                         _cap_origin_ptr,
                         nullptr,
@@ -186,8 +186,8 @@ inline FactorIMU::FactorIMU(const FeatureIMUPtr&    _ftr_ptr,
         dDv_dwb_(_ftr_ptr->getJacobianBias().block(6,3,3,3)),
         dDq_dwb_(_ftr_ptr->getJacobianBias().block(3,3,3,3)),
         dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
-        ab_rate_stdev_(std::static_pointer_cast<SensorIMU>(_ftr_ptr->getCapture()->getSensor())->getAbRateStdev()),
-        wb_rate_stdev_(std::static_pointer_cast<SensorIMU>(_ftr_ptr->getCapture()->getSensor())->getWbRateStdev()),
+        ab_rate_stdev_(std::static_pointer_cast<SensorImu>(_ftr_ptr->getCapture()->getSensor())->getAbRateStdev()),
+        wb_rate_stdev_(std::static_pointer_cast<SensorImu>(_ftr_ptr->getCapture()->getSensor())->getWbRateStdev()),
         sqrt_A_r_dt_inv((Eigen::Matrix3d::Identity() * ab_rate_stdev_ * sqrt(dt_)).inverse()),
         sqrt_W_r_dt_inv((Eigen::Matrix3d::Identity() * wb_rate_stdev_ * sqrt(dt_)).inverse())
 {
@@ -195,7 +195,7 @@ inline FactorIMU::FactorIMU(const FeatureIMUPtr&    _ftr_ptr,
 }
 
 template<typename T>
-inline bool FactorIMU::operator ()(const T* const _p1,
+inline bool FactorImu::operator ()(const T* const _p1,
                                    const T* const _q1,
                                    const T* const _v1,
                                    const T* const _b1,
@@ -227,7 +227,7 @@ inline bool FactorIMU::operator ()(const T* const _p1,
     return true;
 }
 
-Eigen::Vector9d FactorIMU::error()
+Eigen::Vector9d FactorImu::error()
 {
     Vector6d bias = capture_other_ptr_.lock()->getCalibration();
 
@@ -252,7 +252,7 @@ Eigen::Vector9d FactorIMU::error()
 }
 
 template<typename D1, typename D2, typename D3>
-inline bool FactorIMU::residual(const Eigen::MatrixBase<D1> &       _p1,
+inline bool FactorImu::residual(const Eigen::MatrixBase<D1> &       _p1,
                                 const Eigen::QuaternionBase<D2> &   _q1,
                                 const Eigen::MatrixBase<D1> &       _v1,
                                 const Eigen::MatrixBase<D1> &       _ab1,
@@ -265,7 +265,7 @@ inline bool FactorIMU::residual(const Eigen::MatrixBase<D1> &       _p1,
                                 Eigen::MatrixBase<D3> &             _res) const
 {
 
-    /*  Help for the IMU residual function
+    /*  Help for the Imu residual function
      *
      *  Notations:
      *    D_* : a motion in the Delta manifold                           -- implemented as 10-vectors with [Dp, Dq, Dv]
@@ -428,17 +428,17 @@ inline bool FactorIMU::residual(const Eigen::MatrixBase<D1> &       _p1,
     return true;
 }
 
-inline Eigen::VectorXd FactorIMU::expectation() const
+inline Eigen::VectorXd FactorImu::expectation() const
 {
     FrameBasePtr frm_current = getFeature()->getFrame();
     FrameBasePtr frm_previous = getFrameOther();
 
-    //get information on current_frame in the FactorIMU
+    //get information on current_frame in the FactorImu
     const Eigen::Vector3d frame_current_pos    = (frm_current->getP()->getState());
     const Eigen::Quaterniond frame_current_ori   (frm_current->getO()->getState().data());
     const Eigen::Vector3d frame_current_vel    = (frm_current->getV()->getState());
 
-    // get info on previous frame in the FactorIMU
+    // get info on previous frame in the FactorImu
     const Eigen::Vector3d frame_previous_pos   = (frm_previous->getP()->getState());
     const Eigen::Quaterniond frame_previous_ori  (frm_previous->getO()->getState().data());
     const Eigen::Vector3d frame_previous_vel   = (frm_previous->getV()->getState());
@@ -458,7 +458,7 @@ inline Eigen::VectorXd FactorIMU::expectation() const
 }
 
 template<typename D1, typename D2, typename D3, typename D4>
-inline void FactorIMU::expectation(const Eigen::MatrixBase<D1> &        _p1,
+inline void FactorImu::expectation(const Eigen::MatrixBase<D1> &        _p1,
                                        const Eigen::QuaternionBase<D2> &    _q1,
                                        const Eigen::MatrixBase<D1> &        _v1,
                                        const Eigen::MatrixBase<D1> &        _p2,
@@ -480,7 +480,7 @@ inline void FactorIMU::expectation(const Eigen::MatrixBase<D1> &        _p1,
  * Optimization results
  * ================================================
  *
- * Using gtest_IMU.cpp
+ * Using gtest_Imu.cpp
  *
  * Conclusion: Residuals with method 1 and 2 are essentially identical, after exactly the same number of iterations.
  *
@@ -488,48 +488,48 @@ inline void FactorIMU::expectation(const Eigen::MatrixBase<D1> &        _p1,
  *
  * With Method 1:
  *
-[ RUN      ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
-[trace][10:58:16] [gtest_IMU.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final cost: 1.251480e-06, Termination: CONVERGENCE
-[trace][10:58:16] [gtest_IMU.cpp L490 : TestBody] w * DT (rather be lower than 1.57 approx) = 0.5   1 1.5
-[       OK ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (53 ms)
-[ RUN      ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2
-[trace][10:58:16] [gtest_IMU.cpp L564 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.238969e+03, Final cost: 1.059256e-19, Termination: CONVERGENCE
-[       OK ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (56 ms)
-[ RUN      ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2
-[trace][10:58:16] [gtest_IMU.cpp L638 : TestBody] Ceres Solver Report: Iterations: 17, Initial cost: 4.769588e+03, Final cost: 3.767740e-19, Termination: CONVERGENCE
-[       OK ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (50 ms)
-[----------] 3 tests from Process_Factor_IMU (159 ms total)
-
-[----------] 2 tests from Process_Factor_IMU_ODO
-[ RUN      ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
-[trace][10:58:16] [gtest_IMU.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03, Final cost: 1.867678e-22, Termination: CONVERGENCE
-[       OK ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms)
-[ RUN      ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0
-[trace][10:58:16] [gtest_IMU.cpp L783 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.363681e+04, Final cost: 1.879880e-20, Termination: CONVERGENCE
-[       OK ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (52 ms)
-[----------] 2 tests from Process_Factor_IMU_ODO (120 ms total)
+[ RUN      ] Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
+[trace][10:58:16] [gtest_Imu.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final cost: 1.251480e-06, Termination: CONVERGENCE
+[trace][10:58:16] [gtest_Imu.cpp L490 : TestBody] w * DT (rather be lower than 1.57 approx) = 0.5   1 1.5
+[       OK ] Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (53 ms)
+[ RUN      ] Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2
+[trace][10:58:16] [gtest_Imu.cpp L564 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.238969e+03, Final cost: 1.059256e-19, Termination: CONVERGENCE
+[       OK ] Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (56 ms)
+[ RUN      ] Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2
+[trace][10:58:16] [gtest_Imu.cpp L638 : TestBody] Ceres Solver Report: Iterations: 17, Initial cost: 4.769588e+03, Final cost: 3.767740e-19, Termination: CONVERGENCE
+[       OK ] Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (50 ms)
+[----------] 3 tests from Process_Factor_Imu (159 ms total)
+
+[----------] 2 tests from Process_Factor_Imu_ODO
+[ RUN      ] Process_Factor_Imu_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
+[trace][10:58:16] [gtest_Imu.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03, Final cost: 1.867678e-22, Termination: CONVERGENCE
+[       OK ] Process_Factor_Imu_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms)
+[ RUN      ] Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0
+[trace][10:58:16] [gtest_Imu.cpp L783 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.363681e+04, Final cost: 1.879880e-20, Termination: CONVERGENCE
+[       OK ] Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (52 ms)
+[----------] 2 tests from Process_Factor_Imu_ODO (120 ms total)
 *
 * With Method 2:
 *
-[ RUN      ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
-[trace][11:15:43] [gtest_IMU.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final cost: 1.251479e-06, Termination: CONVERGENCE
-[trace][11:15:43] [gtest_IMU.cpp L490 : TestBody] w * DT (rather be lower than 1.57 approx) = 0.5   1 1.5
-[       OK ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (37 ms)
-[ RUN      ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2
-[trace][11:15:43] [gtest_IMU.cpp L564 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.238985e+03, Final cost: 1.058935e-19, Termination: CONVERGENCE
-[       OK ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (48 ms)
-[ RUN      ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2
-[trace][11:15:43] [gtest_IMU.cpp L638 : TestBody] Ceres Solver Report: Iterations: 17, Initial cost: 4.769603e+03, Final cost: 3.762091e-19, Termination: CONVERGENCE
-[       OK ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (47 ms)
-[----------] 3 tests from Process_Factor_IMU (133 ms total)
-
-[----------] 2 tests from Process_Factor_IMU_ODO
-[ RUN      ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
-[trace][11:15:43] [gtest_IMU.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03, Final cost: 1.855814e-22, Termination: CONVERGENCE
-[       OK ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms)
-[ RUN      ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0
-[trace][11:15:43] [gtest_IMU.cpp L783 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.363675e+04, Final cost: 1.880084e-20, Termination: CONVERGENCE
-[       OK ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (59 ms)
-[----------] 2 tests from Process_Factor_IMU_ODO (127 ms total)
+[ RUN      ] Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
+[trace][11:15:43] [gtest_Imu.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final cost: 1.251479e-06, Termination: CONVERGENCE
+[trace][11:15:43] [gtest_Imu.cpp L490 : TestBody] w * DT (rather be lower than 1.57 approx) = 0.5   1 1.5
+[       OK ] Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (37 ms)
+[ RUN      ] Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2
+[trace][11:15:43] [gtest_Imu.cpp L564 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.238985e+03, Final cost: 1.058935e-19, Termination: CONVERGENCE
+[       OK ] Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (48 ms)
+[ RUN      ] Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2
+[trace][11:15:43] [gtest_Imu.cpp L638 : TestBody] Ceres Solver Report: Iterations: 17, Initial cost: 4.769603e+03, Final cost: 3.762091e-19, Termination: CONVERGENCE
+[       OK ] Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (47 ms)
+[----------] 3 tests from Process_Factor_Imu (133 ms total)
+
+[----------] 2 tests from Process_Factor_Imu_ODO
+[ RUN      ] Process_Factor_Imu_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
+[trace][11:15:43] [gtest_Imu.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03, Final cost: 1.855814e-22, Termination: CONVERGENCE
+[       OK ] Process_Factor_Imu_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms)
+[ RUN      ] Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0
+[trace][11:15:43] [gtest_Imu.cpp L783 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.363675e+04, Final cost: 1.880084e-20, Termination: CONVERGENCE
+[       OK ] Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (59 ms)
+[----------] 2 tests from Process_Factor_Imu_ODO (127 ms total)
 *
 */
diff --git a/include/IMU/feature/feature_IMU.h b/include/imu/feature/feature_imu.h
similarity index 77%
rename from include/IMU/feature/feature_IMU.h
rename to include/imu/feature/feature_imu.h
index 6497213dc85c6996930208a48d64b479152c34a8..c4ecb458aa0621addc30e540c73a6f0e44a92fec 100644
--- a/include/IMU/feature/feature_IMU.h
+++ b/include/imu/feature/feature_imu.h
@@ -2,7 +2,7 @@
 #define FEATURE_IMU_H_
 
 //Wolf includes
-#include "IMU/capture/capture_IMU.h"
+#include "imu/capture/capture_imu.h"
 #include "core/feature/feature_base.h"
 #include "core/common/wolf.h"
 
@@ -10,10 +10,10 @@
 
 namespace wolf {
 
-//WOLF_PTR_TYPEDEFS(CaptureIMU);
-WOLF_PTR_TYPEDEFS(FeatureIMU);
+//WOLF_PTR_TYPEDEFS(CaptureImu);
+WOLF_PTR_TYPEDEFS(FeatureImu);
 
-class FeatureIMU : public FeatureBase
+class FeatureImu : public FeatureBase
 {
     public:
 
@@ -21,12 +21,12 @@ class FeatureIMU : public FeatureBase
          *
          * \param _measurement the measurement
          * \param _meas_covariance the noise of the measurement
-         * \param _dD_db_jacobians Jacobians of preintegrated delta wrt IMU biases
+         * \param _dD_db_jacobians Jacobians of preintegrated delta wrt Imu biases
          * \param acc_bias accelerometer bias of origin frame
          * \param gyro_bias gyroscope bias of origin frame
          * \param _cap_imu_ptr pointer to parent CaptureMotion
          */
-        FeatureIMU(const Eigen::VectorXd& _delta_preintegrated,
+        FeatureImu(const Eigen::VectorXd& _delta_preintegrated,
                    const Eigen::MatrixXd& _delta_preintegrated_covariance,
                    const Eigen::Vector6d& _bias,
                    const Eigen::Matrix<double,9,6>& _dD_db_jacobians,
@@ -36,9 +36,9 @@ class FeatureIMU : public FeatureBase
          *
          * \param _cap_imu_ptr pointer to parent CaptureMotion
          */
-        FeatureIMU(CaptureMotionPtr _cap_imu_ptr);
+        FeatureImu(CaptureMotionPtr _cap_imu_ptr);
 
-        virtual ~FeatureIMU();
+        virtual ~FeatureImu();
 
         const Eigen::Vector3d&              getAccBiasPreint()  const;
         const Eigen::Vector3d&              getGyroBiasPreint() const;
@@ -56,17 +56,17 @@ class FeatureIMU : public FeatureBase
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 };
 
-inline const Eigen::Vector3d& FeatureIMU::getAccBiasPreint() const
+inline const Eigen::Vector3d& FeatureImu::getAccBiasPreint() const
 {
     return acc_bias_preint_;
 }
 
-inline const Eigen::Vector3d& FeatureIMU::getGyroBiasPreint() const
+inline const Eigen::Vector3d& FeatureImu::getGyroBiasPreint() const
 {
     return gyro_bias_preint_;
 }
 
-inline const Eigen::Matrix<double, 9, 6>& FeatureIMU::getJacobianBias() const
+inline const Eigen::Matrix<double, 9, 6>& FeatureImu::getJacobianBias() const
 {
     return jacobian_bias_;
 }
diff --git a/include/IMU/math/IMU_tools.h b/include/imu/math/imu_tools.h
similarity index 97%
rename from include/IMU/math/IMU_tools.h
rename to include/imu/math/imu_tools.h
index 76a6f5cef00e14e8035204e4f0bd9a4f9e4622a5..5df0828e7a9702531231e07de358b59dab4e0852 100644
--- a/include/IMU/math/IMU_tools.h
+++ b/include/imu/math/imu_tools.h
@@ -14,9 +14,9 @@
 /*
  * Most functions in this file are explained in the document:
  *
- *   Joan Sola, "IMU pre-integration", 2015-2017 IRI-CSIC
+ *   Joan Sola, "Imu pre-integration", 2015-2017 IRI-CSIC
  *
- * They relate manipulations of Delta motion magnitudes used for IMU pre-integration.
+ * They relate manipulations of Delta motion magnitudes used for Imu pre-integration.
  *
  * The Delta is defined as
  *     Delta = [Dp, Dq, Dv]
@@ -34,9 +34,9 @@
  *   - composeOverState: x2 = x1 (+) D
  *   - betweenStates: D = x2 (-) x1, so that x2 = x1 (+) D
  *   - log: got from Delta manifold to tangent space (equivalent to log() in rotations)
- *   - exp_IMU: go from tangent space to delta manifold (equivalent to exp() in rotations)
- *   - plus: D2 = D1 (+) exp_IMU(d)
- *   - diff: d = log_IMU( D2 (-) D1 )
+ *   - exp_Imu: go from tangent space to delta manifold (equivalent to exp() in rotations)
+ *   - plus: D2 = D1 (+) exp_Imu(d)
+ *   - diff: d = log_Imu( D2 (-) D1 )
  *   - body2delta: construct a delta from body magnitudes of linAcc and angVel
  */
 
@@ -340,7 +340,7 @@ inline Matrix<typename D1::Scalar, 10, 1> betweenStates(const MatrixBase<D1>& x1
 }
 
 template<typename Derived>
-Matrix<typename Derived::Scalar, 9, 1> log_IMU(const MatrixBase<Derived>& delta_in)
+Matrix<typename Derived::Scalar, 9, 1> log_Imu(const MatrixBase<Derived>& delta_in)
 {
     MatrixSizeCheck<10, 1>::check(delta_in);
 
@@ -361,7 +361,7 @@ Matrix<typename Derived::Scalar, 9, 1> log_IMU(const MatrixBase<Derived>& delta_
 }
 
 template<typename Derived>
-Matrix<typename Derived::Scalar, 10, 1> exp_IMU(const MatrixBase<Derived>& d_in)
+Matrix<typename Derived::Scalar, 10, 1> exp_Imu(const MatrixBase<Derived>& d_in)
 {
     MatrixSizeCheck<9, 1>::check(d_in);
 
@@ -578,13 +578,13 @@ void motion2data(const MatrixBase<D1>& a, const MatrixBase<D2>& w, const Quatern
     w_m = w + w_b;
 }
 
-/* Create IMU data from body motion
+/* Create Imu data from body motion
  * Input:
  *   motion : [ax, ay, az, wx, wy, wz] the motion in body frame
  *   q      : the current orientation wrt horizontal
- *   bias   : the bias of the IMU
+ *   bias   : the bias of the Imu
  * Output:
- *   return : the data vector as created by the IMU (with motion, gravity, and bias)
+ *   return : the data vector as created by the Imu (with motion, gravity, and bias)
  */
 template<typename D1, typename D2, typename D3>
 Matrix<typename D1::Scalar, 6, 1> motion2data(const MatrixBase<D1>& motion, const QuaternionBase<D2>& q, const MatrixBase<D3>& bias)
@@ -607,4 +607,4 @@ Matrix<typename D1::Scalar, 6, 1> motion2data(const MatrixBase<D1>& motion, cons
 } // namespace imu
 } // namespace wolf
 
-#endif /* IMU_TOOLS_H_ */
+#endif /* Imu_TOOLS_H_ */
diff --git a/include/IMU/processor/processor_IMU.h b/include/imu/processor/processor_imu.h
similarity index 78%
rename from include/IMU/processor/processor_IMU.h
rename to include/imu/processor/processor_imu.h
index 7a6a7f9a168c9fb4bb11586f4a4e8a39d7b1c6b6..bf3d20d7a08c22be8256e38a502d0c5e0aacea68 100644
--- a/include/IMU/processor/processor_IMU.h
+++ b/include/imu/processor/processor_imu.h
@@ -2,20 +2,17 @@
 #define PROCESSOR_IMU_H
 
 // Wolf
-#include "IMU/capture/capture_IMU.h"
-#include "IMU/feature/feature_IMU.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/feature/feature_imu.h"
 #include "core/processor/processor_motion.h"
 
 namespace wolf {
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsIMU);
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsImu);
 
-struct ProcessorParamsIMU : public ProcessorParamsMotion
+struct ProcessorParamsImu : public ProcessorParamsMotion
 {
-  // ProcessorParamsIMU() = default;
-    // using ProcessorParamsMotion::ProcessorParamsMotion;
-
-    ProcessorParamsIMU() = default;
-    ProcessorParamsIMU(std::string _unique_name, const ParamsServer& _server):
+    ProcessorParamsImu() = default;
+    ProcessorParamsImu(std::string _unique_name, const ParamsServer& _server):
         ProcessorParamsMotion(_unique_name, _server)
     {
         //
@@ -26,17 +23,17 @@ struct ProcessorParamsIMU : public ProcessorParamsMotion
     }
 };
 
-WOLF_PTR_TYPEDEFS(ProcessorIMU);
+WOLF_PTR_TYPEDEFS(ProcessorImu);
     
 //class
-class ProcessorIMU : public ProcessorMotion{
+class ProcessorImu : public ProcessorMotion{
     public:
-        ProcessorIMU(ProcessorParamsIMUPtr _params_motion_IMU);
-        virtual ~ProcessorIMU();
-        virtual void configure(SensorBasePtr _sensor) override { };
+        ProcessorImu(ProcessorParamsImuPtr _params_motion_Imu);
+        virtual ~ProcessorImu();
+        // virtual void configure(SensorBasePtr _sensor) override { };
+        virtual void configure(SensorBasePtr _sensor) override;
 
-        // Factory method for high level API
-        static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params);
+        WOLF_PROCESSOR_CREATE(ProcessorImu, ProcessorParamsImu);
 
     protected:
         virtual void computeCurrentDelta(const Eigen::VectorXd& _data,
@@ -75,7 +72,7 @@ class ProcessorIMU : public ProcessorMotion{
                                             CaptureBasePtr _capture_origin) override;
 
     protected:
-        ProcessorParamsIMUPtr params_motion_IMU_;
+        ProcessorParamsImuPtr params_motion_Imu_;
         Eigen::Matrix<double, 9, 9> unmeasured_perturbation_cov_;
 
 };
@@ -88,11 +85,11 @@ class ProcessorIMU : public ProcessorMotion{
 
 namespace wolf{
 
-inline Eigen::VectorXd ProcessorIMU::deltaZero() const
+inline Eigen::VectorXd ProcessorImu::deltaZero() const
 {
     return (Eigen::VectorXd(10) << 0,0,0,  0,0,0,1,  0,0,0 ).finished(); // p, q, v
 }
 
 } // namespace wolf
 
-#endif // PROCESSOR_IMU_H
+#endif // PROCESSOR_Imu_H
diff --git a/include/imu/sensor/sensor_imu.h b/include/imu/sensor/sensor_imu.h
new file mode 100644
index 0000000000000000000000000000000000000000..45cd49c7d1c233e6592e9221ae5fd26bf3a6e053
--- /dev/null
+++ b/include/imu/sensor/sensor_imu.h
@@ -0,0 +1,119 @@
+#ifndef SENSOR_IMU_H
+#define SENSOR_IMU_H
+
+//wolf includes
+#include "core/sensor/sensor_base.h"
+#include "core/utils/params_server.hpp"
+#include <iostream>
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorImu);
+
+
+struct ParamsSensorImu : public ParamsSensorBase
+{
+    //noise std dev
+    double w_noise = 0.001; //standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
+    double a_noise = 0.04; //standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s)
+
+    //Initial biases std dev
+    double ab_initial_stdev = 0.01; //accelerometer micro_g/sec
+    double wb_initial_stdev = 0.01; //gyroscope rad/sec
+
+    // bias rate of change std dev
+    double ab_rate_stdev = 0.00001;
+    double wb_rate_stdev = 0.00001;
+
+    virtual ~ParamsSensorImu() = default;
+    ParamsSensorImu()
+    {
+        //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
+    }
+    ParamsSensorImu(std::string _unique_name, const ParamsServer& _server):
+        ParamsSensorBase(_unique_name, _server)
+    {
+        w_noise             = _server.getParam<double>(prefix + _unique_name + "/w_noise");
+        a_noise             = _server.getParam<double>(prefix + _unique_name + "/a_noise");
+        ab_initial_stdev    = _server.getParam<double>(prefix + _unique_name + "/ab_initial_stdev");
+        wb_initial_stdev    = _server.getParam<double>(prefix + _unique_name + "/wb_initial_stdev");
+        ab_rate_stdev       = _server.getParam<double>(prefix + _unique_name + "/ab_rate_stdev");
+        wb_rate_stdev       = _server.getParam<double>(prefix + _unique_name + "/wb_rate_stdev");
+    }
+    std::string print() const
+    {
+        return "\n" + ParamsSensorBase::print()                           + "\n"
+            + "w_noise: "           + std::to_string(w_noise)           + "\n"
+            + "a_noise: "           + std::to_string(a_noise)           + "\n"
+            + "ab_initial_stdev: "  + std::to_string(ab_initial_stdev)  + "\n"
+            + "wb_initial_stdev: "  + std::to_string(wb_initial_stdev)  + "\n"
+            + "ab_rate_stdev: "     + std::to_string(ab_rate_stdev)     + "\n"
+            + "wb_rate_stdev: "     + std::to_string(wb_rate_stdev)     + "\n";
+    }
+};
+
+WOLF_PTR_TYPEDEFS(SensorImu);
+
+class SensorImu : public SensorBase
+{
+
+    protected:
+        double a_noise; //Power Spectral Density (same for all the axis) in micro_g/ sqrt(Hz)
+        double w_noise; //Rate Noise Spectral Density (same for all the axis) in deg/sec/ sqrt(Hz)
+
+        //This is a trial to factor how much can the bias change in 1 sec at most
+        double ab_initial_stdev; //accelerometer m/sec^s
+        double wb_initial_stdev; //gyroscope rad/sec
+        double ab_rate_stdev;    //accelerometer m/sec^2 / sqrt(sec)
+        double wb_rate_stdev;    //gyroscope rad/sec / sqrt(sec)
+
+    public:
+
+        SensorImu(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu& _params);
+        SensorImu(const Eigen::VectorXd& _extrinsics, ParamsSensorImuPtr _params);
+        WOLF_SENSOR_CREATE(SensorImu, ParamsSensorImu, 7);
+
+        double getGyroNoise() const;
+        double getAccelNoise() const;
+        double getWbInitialStdev() const;
+        double getAbInitialStdev() const;
+        double getWbRateStdev() const;
+        double getAbRateStdev() const;
+
+        virtual ~SensorImu();
+
+};
+
+inline double SensorImu::getGyroNoise() const
+{
+    return w_noise;
+}
+
+inline double SensorImu::getAccelNoise() const
+{
+    return a_noise;
+}
+
+inline double SensorImu::getWbInitialStdev() const
+{
+    return wb_initial_stdev;
+}
+
+inline double SensorImu::getAbInitialStdev() const
+{
+    return ab_initial_stdev;
+}
+
+inline double SensorImu::getWbRateStdev() const
+{
+    return wb_rate_stdev;
+}
+
+inline double SensorImu::getAbRateStdev() const
+{
+    return ab_rate_stdev;
+}
+
+} // namespace wolf
+
+#endif // SENSOR_Imu_H
diff --git a/src/capture/capture_IMU.cpp b/src/capture/capture_imu.cpp
similarity index 83%
rename from src/capture/capture_IMU.cpp
rename to src/capture/capture_imu.cpp
index dfdc45a93e3e1357f6fb701cec6aaf0ff713a36e..90d80f0a8275243d99696891cb70708f114e4fd8 100644
--- a/src/capture/capture_IMU.cpp
+++ b/src/capture/capture_imu.cpp
@@ -1,10 +1,10 @@
-#include "IMU/capture/capture_IMU.h"
-#include "IMU/sensor/sensor_IMU.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/sensor/sensor_imu.h"
 #include "core/state_block/state_quaternion.h"
 
 namespace wolf {
 
-CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
+CaptureImu::CaptureImu(const TimeStamp& _init_ts,
                        SensorBasePtr _sensor_ptr,
                        const Eigen::Vector6d& _acc_gyro_data,
                        const Eigen::MatrixXd& _data_cov,
@@ -14,7 +14,7 @@ CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
     //
 }
 
-CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
+CaptureImu::CaptureImu(const TimeStamp& _init_ts,
                        SensorBasePtr _sensor_ptr,
                        const Eigen::Vector6d& _acc_gyro_data,
                        const Eigen::MatrixXd& _data_cov,
@@ -25,7 +25,7 @@ CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
     //
 }
 
-CaptureIMU::~CaptureIMU()
+CaptureImu::~CaptureImu()
 {
     //
 }
diff --git a/src/feature/feature_IMU.cpp b/src/feature/feature_imu.cpp
similarity index 82%
rename from src/feature/feature_IMU.cpp
rename to src/feature/feature_imu.cpp
index cf1773d645c2532caadd3d7b8af664260b84cf3e..d3547a52c2c6794ed2d795af780ca2063ef14e36 100644
--- a/src/feature/feature_IMU.cpp
+++ b/src/feature/feature_imu.cpp
@@ -1,8 +1,8 @@
-#include "IMU/feature/feature_IMU.h"
+#include "imu/feature/feature_imu.h"
 
 namespace wolf {
 
-FeatureIMU::FeatureIMU(const Eigen::VectorXd& _delta_preintegrated,
+FeatureImu::FeatureImu(const Eigen::VectorXd& _delta_preintegrated,
                        const Eigen::MatrixXd& _delta_preintegrated_covariance,
                        const Eigen::Vector6d& _bias,
                        const Eigen::Matrix<double,9,6>& _dD_db_jacobians,
@@ -14,7 +14,7 @@ FeatureIMU::FeatureIMU(const Eigen::VectorXd& _delta_preintegrated,
 {
 }
 
-FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr):
+FeatureImu::FeatureImu(CaptureMotionPtr _cap_imu_ptr):
         FeatureBase("FeatureImu", _cap_imu_ptr->getDeltaPreint(), _cap_imu_ptr->getDeltaPreintCov()),
         acc_bias_preint_ (_cap_imu_ptr->getCalibrationPreint().head<3>()),
         gyro_bias_preint_(_cap_imu_ptr->getCalibrationPreint().tail<3>()),
@@ -22,7 +22,7 @@ FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr):
 {
 }
 
-FeatureIMU::~FeatureIMU()
+FeatureImu::~FeatureImu()
 {
     //
 }
diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_imu.cpp
similarity index 79%
rename from src/processor/processor_IMU.cpp
rename to src/processor/processor_imu.cpp
index 2aa73c0330dcfc4aa47008da96f0d6109f0831ce..a420c0018bb8f48363fa0a04a9a42bfcc1eb2188 100644
--- a/src/processor/processor_IMU.cpp
+++ b/src/processor/processor_imu.cpp
@@ -1,54 +1,49 @@
 // wolf
-#include "IMU/processor/processor_IMU.h"
-#include "IMU/factor/factor_IMU.h"
-#include "IMU/math/IMU_tools.h"
+#include "imu/processor/processor_imu.h"
+#include "imu/factor/factor_imu.h"
+#include "imu/math/imu_tools.h"
 
 namespace wolf {
 
-ProcessorIMU::ProcessorIMU(ProcessorParamsIMUPtr _params_motion_IMU) :
-        ProcessorMotion("ProcessorImu", "POV", 10, 10, 9, 6, 6, _params_motion_IMU),
-        params_motion_IMU_(std::make_shared<ProcessorParamsIMU>(*_params_motion_IMU))
+ProcessorImu::ProcessorImu(ProcessorParamsImuPtr _params_motion_imu) :
+        ProcessorMotion("ProcessorImu", "POV", 3, 10, 10, 9, 6, 6, _params_motion_imu),
+        params_motion_Imu_(std::make_shared<ProcessorParamsImu>(*_params_motion_imu))
 {
     // Set constant parts of Jacobians
     jacobian_delta_preint_.setIdentity(9,9);                                    // dDp'/dDp, dDv'/dDv, all zeros
     jacobian_delta_.setIdentity(9,9);                                           //
     jacobian_calib_.setZero(9,6);
-    unmeasured_perturbation_cov_ = pow(params_motion_IMU_->unmeasured_perturbation_std, 2.0) * Eigen::Matrix<double, 9, 9>::Identity();
+    unmeasured_perturbation_cov_ = pow(params_motion_Imu_->unmeasured_perturbation_std, 2.0) * Eigen::Matrix<double, 9, 9>::Identity();
 }
 
-ProcessorIMU::~ProcessorIMU()
+ProcessorImu::~ProcessorImu()
 {
     //
 }
 
-ProcessorBasePtr ProcessorIMU::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params)
+void ProcessorImu::configure(SensorBasePtr _sensor)
 {
-    auto prc_imu_params = std::static_pointer_cast<ProcessorParamsIMU>(_params);
-
-    auto prc_ptr = std::make_shared<ProcessorIMU>(prc_imu_params);
-
-    prc_ptr->setName(_unique_name);
-
-    return prc_ptr;
+    auto sensor_       = std::dynamic_pointer_cast<SensorImu>(_sensor);
+    assert(sensor_ != nullptr && "Sensor is not of type SensorImu");
 }
 
-bool ProcessorIMU::voteForKeyFrame() const
+bool ProcessorImu::voteForKeyFrame() const
 {
     // time span
-    if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > params_motion_IMU_->max_time_span)
+    if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > params_motion_Imu_->max_time_span)
     {
         WOLF_DEBUG( "PM: vote: time span" );
         return true;
     }
     // buffer length
-    if (getBuffer().get().size() > params_motion_IMU_->max_buff_length)
+    if (getBuffer().get().size() > params_motion_Imu_->max_buff_length)
     {
         WOLF_DEBUG( "PM: vote: buffer length" );
         return true;
     }
     // angle turned
     double angle = 2.0 * asin(delta_integrated_.segment(3,3).norm());
-    if (angle > params_motion_IMU_->angle_turned)
+    if (angle > params_motion_Imu_->angle_turned)
     {
         WOLF_DEBUG( "PM: vote: angle turned" );
         return true;
@@ -58,7 +53,7 @@ bool ProcessorIMU::voteForKeyFrame() const
 }
 
 
-CaptureMotionPtr ProcessorIMU::emplaceCapture(const FrameBasePtr& _frame_own,
+CaptureMotionPtr ProcessorImu::emplaceCapture(const FrameBasePtr& _frame_own,
                                               const SensorBasePtr& _sensor,
                                               const TimeStamp& _ts,
                                               const VectorXd& _data,
@@ -67,7 +62,7 @@ CaptureMotionPtr ProcessorIMU::emplaceCapture(const FrameBasePtr& _frame_own,
                                               const VectorXd& _calib_preint,
                                               const CaptureBasePtr& _capture_origin)
 {
-    auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureIMU>(_frame_own,
+    auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureImu>(_frame_own,
                                                                                                _ts,
                                                                                                _sensor,
                                                                                                _data,
@@ -79,9 +74,9 @@ CaptureMotionPtr ProcessorIMU::emplaceCapture(const FrameBasePtr& _frame_own,
     return cap_motion;
 }
 
-FeatureBasePtr ProcessorIMU::emplaceFeature(CaptureMotionPtr _capture_motion)
+FeatureBasePtr ProcessorImu::emplaceFeature(CaptureMotionPtr _capture_motion)
 {
-    auto feature = FeatureBase::emplace<FeatureIMU>(_capture_motion,
+    auto feature = FeatureBase::emplace<FeatureImu>(_capture_motion,
                                                     _capture_motion->getBuffer().get().back().delta_integr_,
                                                     _capture_motion->getBuffer().get().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
                                                     _capture_motion->getCalibrationPreint(),
@@ -89,17 +84,17 @@ FeatureBasePtr ProcessorIMU::emplaceFeature(CaptureMotionPtr _capture_motion)
     return feature;
 }
 
-FactorBasePtr ProcessorIMU::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
+FactorBasePtr ProcessorImu::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
 {
-    CaptureIMUPtr cap_imu = std::static_pointer_cast<CaptureIMU>(_capture_origin);
-    FeatureIMUPtr ftr_imu = std::static_pointer_cast<FeatureIMU>(_feature_motion);
+    CaptureImuPtr cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin);
+    FeatureImuPtr ftr_imu = std::static_pointer_cast<FeatureImu>(_feature_motion);
 
-    auto fac_imu = FactorBase::emplace<FactorIMU>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
+    auto fac_imu = FactorBase::emplace<FactorImu>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
 
     return fac_imu;
 }
 
-void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXd& _data,
+void ProcessorImu::computeCurrentDelta(const Eigen::VectorXd& _data,
                                        const Eigen::MatrixXd& _data_cov,
                                        const Eigen::VectorXd& _calib,
                                        const double _dt,
@@ -141,7 +136,7 @@ void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXd& _data,
 
 }
 
-void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+void ProcessorImu::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
                                   const Eigen::VectorXd& _delta,
                                   const double _dt,
                                   Eigen::VectorXd& _delta_preint_plus_delta) const
@@ -156,7 +151,7 @@ void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
     _delta_preint_plus_delta = imu::compose(_delta_preint, _delta, _dt);
 }
 
-void ProcessorIMU::statePlusDelta(const Eigen::VectorXd& _x,
+void ProcessorImu::statePlusDelta(const Eigen::VectorXd& _x,
                                   const Eigen::VectorXd& _delta,
                                   const double _dt,
                                   Eigen::VectorXd& _x_plus_delta) const
@@ -169,7 +164,7 @@ void ProcessorIMU::statePlusDelta(const Eigen::VectorXd& _x,
     _x_plus_delta = imu::composeOverState(_x, _delta, _dt);
 }
 
-void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+void ProcessorImu::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
                                   const Eigen::VectorXd& _delta,
                                   const double _dt,
                                   Eigen::VectorXd& _delta_preint_plus_delta,
@@ -210,5 +205,6 @@ void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
 
 namespace wolf
 {
-WOLF_REGISTER_PROCESSOR("ProcessorIMU", ProcessorIMU)
+WOLF_REGISTER_PROCESSOR("ProcessorImu", ProcessorImu)
+WOLF_REGISTER_PROCESSOR_AUTO("ProcessorImu", ProcessorImu)
 }
diff --git a/src/sensor/sensor_IMU.cpp b/src/sensor/sensor_IMU.cpp
deleted file mode 100644
index 1036b78214a0ee9d4d15c2ab8f0b677f0da96d6f..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_IMU.cpp
+++ /dev/null
@@ -1,49 +0,0 @@
-#include "IMU/sensor/sensor_IMU.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-
-namespace wolf {
-
-SensorIMU::SensorIMU(const Eigen::VectorXd& _extrinsics, ParamsSensorIMUPtr _params) :
-        SensorIMU(_extrinsics, *_params)
-{
-    //
-}
-
-SensorIMU::SensorIMU(const Eigen::VectorXd& _extrinsics, const ParamsSensorIMU& _params) :
-        SensorBase("SensorIMU", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6d()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, false, true),
-        a_noise(_params.a_noise),
-        w_noise(_params.w_noise),
-        ab_initial_stdev(_params.ab_initial_stdev),
-        wb_initial_stdev(_params.wb_initial_stdev),
-        ab_rate_stdev(_params.ab_rate_stdev),
-        wb_rate_stdev(_params.wb_rate_stdev)
-{
-    assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2D.");
-}
-
-SensorIMU::~SensorIMU()
-{
-    //
-}
-
-// Define the factory method
-SensorBasePtr SensorIMU::create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics_pq,
-                              const ParamsSensorBasePtr _intrinsics)
-{
-    // decode extrinsics vector
-    assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
-
-    ParamsSensorIMUPtr params = std::static_pointer_cast<ParamsSensorIMU>(_intrinsics);
-    SensorIMUPtr sen = std::make_shared<SensorIMU>(_extrinsics_pq, params);
-    sen->setName(_unique_name);
-    return sen;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("SensorIMU", SensorIMU)
-} // namespace wolf
diff --git a/src/sensor/sensor_imu.cpp b/src/sensor/sensor_imu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..927a1aea07db217557ccba5739f0df4ec020b581
--- /dev/null
+++ b/src/sensor/sensor_imu.cpp
@@ -0,0 +1,37 @@
+#include "imu/sensor/sensor_imu.h"
+#include "core/state_block/state_block.h"
+#include "core/state_block/state_quaternion.h"
+
+namespace wolf {
+
+SensorImu::SensorImu(const Eigen::VectorXd& _extrinsics, ParamsSensorImuPtr _params) :
+        SensorImu(_extrinsics, *_params)
+{
+    //
+}
+
+SensorImu::SensorImu(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu& _params) :
+        SensorBase("SensorImu", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6d()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, false, true),
+        a_noise(_params.a_noise),
+        w_noise(_params.w_noise),
+        ab_initial_stdev(_params.ab_initial_stdev),
+        wb_initial_stdev(_params.wb_initial_stdev),
+        ab_rate_stdev(_params.ab_rate_stdev),
+        wb_rate_stdev(_params.wb_rate_stdev)
+{
+    assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2d.");
+}
+
+SensorImu::~SensorImu()
+{
+    //
+}
+
+} // namespace wolf
+
+// Register in the SensorFactory
+#include "core/sensor/sensor_factory.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR("SensorImu", SensorImu)
+WOLF_REGISTER_SENSOR_AUTO("SensorImu", SensorImu);
+} // namespace wolf
diff --git a/src/yaml/processor_IMU_yaml.cpp b/src/yaml/processor_imu_yaml.cpp
similarity index 78%
rename from src/yaml/processor_IMU_yaml.cpp
rename to src/yaml/processor_imu_yaml.cpp
index 681f7243402ac81745651cd88ed065123a482347..03e3e232f989e0127f4d9769474c6c4bcd875be4 100644
--- a/src/yaml/processor_IMU_yaml.cpp
+++ b/src/yaml/processor_imu_yaml.cpp
@@ -6,7 +6,7 @@
  */
 
 // wolf yaml
-#include "IMU/processor/processor_IMU.h"
+#include "imu/processor/processor_imu.h"
 #include "core/yaml/yaml_conversion.h"
 
 // wolf
@@ -20,16 +20,16 @@ namespace wolf
 
 namespace
 {
-static ProcessorParamsBasePtr createProcessorIMUParams(const std::string & _filename_dot_yaml)
+static ProcessorParamsBasePtr createProcessorImuParams(const std::string & _filename_dot_yaml)
 {
     YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
     std::cout << _filename_dot_yaml << '\n';
 
-    if (config["type"].as<std::string>() == "ProcessorIMU")
+    if (config["type"].as<std::string>() == "ProcessorImu")
     {
         YAML::Node kf_vote = config["keyframe_vote"];
 
-        ProcessorParamsIMUPtr params = std::make_shared<ProcessorParamsIMU>();
+        ProcessorParamsImuPtr params = std::make_shared<ProcessorParamsImu>();
         params->time_tolerance = config["time_tolerance"]           .as<double>();
         params->unmeasured_perturbation_std = config["unmeasured_perturbation_std"].as<double>();
 
@@ -47,7 +47,7 @@ static ProcessorParamsBasePtr createProcessorIMUParams(const std::string & _file
 }
 
 // Register in the SensorFactory
-const bool WOLF_UNUSED registered_prc_odom_3D = ProcessorParamsFactory::get().registerCreator("ProcessorIMU", createProcessorIMUParams);
+const bool WOLF_UNUSED registered_prc_odom_3d = ProcessorParamsFactory::get().registerCreator("ProcessorImu", createProcessorImuParams);
 
 } // namespace [unnamed]
 
diff --git a/src/yaml/sensor_IMU_yaml.cpp b/src/yaml/sensor_imu_yaml.cpp
similarity index 81%
rename from src/yaml/sensor_IMU_yaml.cpp
rename to src/yaml/sensor_imu_yaml.cpp
index 89d7c382f48d8202fa439009ba9ff3a5191cb3e9..9268169c6c13835cce8f8f6d656da8910664103d 100644
--- a/src/yaml/sensor_IMU_yaml.cpp
+++ b/src/yaml/sensor_imu_yaml.cpp
@@ -6,7 +6,7 @@
  */
 
 // wolf yaml
-#include "IMU/sensor/sensor_IMU.h"
+#include "imu/sensor/sensor_imu.h"
 #include "core/yaml/yaml_conversion.h"
 
 // wolf
@@ -21,16 +21,16 @@ namespace wolf
 namespace
 {
 
-static ParamsSensorBasePtr createParamsSensorIMU(const std::string & _filename_dot_yaml)
+static ParamsSensorBasePtr createParamsSensorImu(const std::string & _filename_dot_yaml)
 {
     YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
 
-    if (config["type"].as<std::string>() == "SensorIMU")
+    if (config["type"].as<std::string>() == "SensorImu")
     {
         YAML::Node variances        = config["motion_variances"];
         YAML::Node kf_vote          = config["keyframe_vote"];
 
-        ParamsSensorIMUPtr params = std::make_shared<ParamsSensorIMU>();
+        ParamsSensorImuPtr params = std::make_shared<ParamsSensorImu>();
 
         params->a_noise             = variances["a_noise"]          .as<double>();
         params->w_noise             = variances["w_noise"]          .as<double>();
@@ -47,7 +47,7 @@ static ParamsSensorBasePtr createParamsSensorIMU(const std::string & _filename_d
 }
 
 // Register in the SensorFactory
-const bool WOLF_UNUSED registered_imu_intr = ParamsSensorFactory::get().registerCreator("SensorIMU", createParamsSensorIMU);
+const bool WOLF_UNUSED registered_imu_intr = ParamsSensorFactory::get().registerCreator("SensorImu", createParamsSensorImu);
 
 } // namespace [unnamed]
 
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index a6a1d9cd781eeb9d1c3349e7db9794e5c57d1aea..35a9d415d3016f1b4849c398d8ad9aabeff12f85 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -13,23 +13,22 @@ target_link_libraries(gtest_example ${PLUGIN_NAME})      #
 #                                                         #
 ###########################################################
 
-wolf_add_gtest(gtest_processor_IMU gtest_processor_IMU.cpp)
-message("WHAT IS HERE ${PLUGIN_NAME} ${wolf_LIBRARY}")
-target_link_libraries(gtest_processor_IMU ${PLUGIN_NAME} ${wolf_LIBRARY})
+wolf_add_gtest(gtest_processor_imu gtest_processor_imu.cpp)
+target_link_libraries(gtest_processor_imu ${PLUGIN_NAME} ${wolf_LIBRARY})
 
-wolf_add_gtest(gtest_IMU gtest_IMU.cpp)
-target_link_libraries(gtest_IMU ${PLUGIN_NAME} ${wolf_LIBRARY})
+wolf_add_gtest(gtest_imu gtest_imu.cpp)
+target_link_libraries(gtest_imu ${PLUGIN_NAME} ${wolf_LIBRARY})
 
-wolf_add_gtest(gtest_IMU_tools gtest_IMU_tools.cpp)
-target_link_libraries(gtest_IMU_tools ${PLUGIN_NAME} ${wolf_LIBRARY})
+wolf_add_gtest(gtest_imu_tools gtest_imu_tools.cpp)
+target_link_libraries(gtest_imu_tools ${PLUGIN_NAME} ${wolf_LIBRARY})
 
-wolf_add_gtest(gtest_processor_IMU_jacobians gtest_processor_IMU_jacobians.cpp)
-target_link_libraries(gtest_processor_IMU_jacobians ${PLUGIN_NAME} ${wolf_LIBRARY})
+wolf_add_gtest(gtest_processor_imu_jacobians gtest_processor_imu_jacobians.cpp)
+target_link_libraries(gtest_processor_imu_jacobians ${PLUGIN_NAME} ${wolf_LIBRARY})
 
-wolf_add_gtest(gtest_feature_IMU gtest_feature_IMU.cpp)
-target_link_libraries(gtest_feature_IMU ${PLUGIN_NAME} ${wolf_LIBRARY})
+wolf_add_gtest(gtest_feature_imu gtest_feature_imu.cpp)
+target_link_libraries(gtest_feature_imu ${PLUGIN_NAME} ${wolf_LIBRARY})
 
 # Has been excluded from tests for god knows how long, so thing is broken.
 # Maybe call an archeologist to fix this thing?
-# wolf_add_gtest(gtest_factor_IMU gtest_factor_IMU.cpp)
-# target_link_libraries(gtest_factor_IMU ${PLUGIN_NAME} ${wolf_LIBRARY})
+# wolf_add_gtest(gtest_factor_imu gtest_factor_imu.cpp)
+# target_link_libraries(gtest_factor_imu ${PLUGIN_NAME} ${wolf_LIBRARY})
diff --git a/test/gtest_factor_IMU.cpp b/test/gtest_factor_imu.cpp
similarity index 88%
rename from test/gtest_factor_IMU.cpp
rename to test/gtest_factor_imu.cpp
index 2c70a6321acd19833615e20d782341e54bfaad84..f6f26a4aee404d7ea579beece22ebedcbea60112 100644
--- a/test/gtest_factor_IMU.cpp
+++ b/test/gtest_factor_imu.cpp
@@ -8,15 +8,15 @@
 #include <core/utils/utils_gtest.h>
 #include <core/utils/logging.h>
 
-// IMU
+// Imu
 #include "imu/internal/config.h"
-#include "imu/capture/capture_IMU.h"
-#include "imu/processor/processor_IMU.h"
-#include "imu/sensor/sensor_IMU.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/processor/processor_imu.h"
+#include "imu/sensor/sensor_imu.h"
 
 //Wolf
-#include <core/factor/factor_pose_3D.h>
-#include <core/processor/processor_odom_3D.h>
+#include <core/factor/factor_pose_3d.h>
+#include <core/processor/processor_odom_3d.h>
 #include <core/ceres_wrapper/ceres_manager.h>
 
 // debug
@@ -30,22 +30,22 @@ using namespace std;
 using namespace wolf;
 
 /*
- * This test is designed to test IMU biases in a particular case : perfect IMU, not moving.
+ * This test is designed to test Imu biases in a particular case : perfect Imu, not moving.
  * var(b1,b2,p2,v2,q2), inv(p1,q1,v1); fac1: imu+(b1=b2)
  * So there is no odometry data.
- * IMU data file should first contain initial conditions, then the time_step between each new imu and the last one (in seconds) data,
- * and finally the last stateafter integration and the last timestamp, Then it should contain all IMU data and related timestamps
+ * Imu data file should first contain initial conditions, then the time_step between each new imu and the last one (in seconds) data,
+ * and finally the last stateafter integration and the last timestamp, Then it should contain all Imu data and related timestamps
  */
 
-class FactorIMU_biasTest_Static_NullBias : public testing::Test
+class FactorImu_biasTest_Static_NullBias : public testing::Test
 {
     public:
         wolf::TimeStamp t;
-        SensorIMUPtr sen_imu;
+        SensorImuPtr sen_imu;
         ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
         ProcessorBasePtr processor_;
-        ProcessorIMUPtr processor_imu;
+        ProcessorImuPtr processor_imu;
         FrameBasePtr KF0;
         FrameBasePtr KF1;
         Eigen::Vector6d origin_bias;
@@ -72,11 +72,11 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test
         ceres_options.max_num_iterations = 1e4;
         ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
-        // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
-        processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
-        sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
+        // SENSOR + PROCESSOR Imu
+        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu.yaml");
+        sen_imu = std::static_pointer_cast<SensorImu>(sen0_ptr);
+        processor_imu = std::static_pointer_cast<ProcessorImu>(processor_);
     
         //===================================================== END{SETTING PROBLEM}
 
@@ -103,7 +103,7 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test
         Eigen::Vector6d data_imu;
         data_imu << -wolf::gravity(), 0,0,0;
 
-        wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on IMU (measure only gravity here)
+        wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on Imu (measure only gravity here)
 
         for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
         {
@@ -126,15 +126,15 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test
     virtual void TearDown(){}
 };
 
-class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
+class FactorImu_biasTest_Static_NonNullAccBias : public testing::Test
 {
     public:
         wolf::TimeStamp t;
-        SensorIMUPtr sen_imu;
+        SensorImuPtr sen_imu;
         ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
         ProcessorBasePtr processor_;
-        ProcessorIMUPtr processor_imu;
+        ProcessorImuPtr processor_imu;
         FrameBasePtr KF0;
         FrameBasePtr KF1;
         Eigen::Vector6d origin_bias;
@@ -160,11 +160,11 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
         ceres_options.max_num_iterations = 1e4;
         ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
-        // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
-        processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
-        sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
+        // SENSOR + PROCESSOR Imu
+        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu.yaml");
+        sen_imu = std::static_pointer_cast<SensorImu>(sen0_ptr);
+        processor_imu = std::static_pointer_cast<ProcessorImu>(processor_);
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -188,7 +188,7 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
         data_imu << -wolf::gravity(), 0,0,0;
         data_imu = data_imu + origin_bias;
 
-        wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on IMU (measure only gravity here)
+        wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on Imu (measure only gravity here)
 
         for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
         {
@@ -210,15 +210,15 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
     virtual void TearDown(){}
 };
 
-class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
+class FactorImu_biasTest_Static_NonNullGyroBias : public testing::Test
 {
     public:
         wolf::TimeStamp t;
-        SensorIMUPtr sen_imu;
+        SensorImuPtr sen_imu;
         ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
         ProcessorBasePtr processor_;
-        ProcessorIMUPtr processor_imu;
+        ProcessorImuPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
         Eigen::Vector6d origin_bias;
@@ -244,11 +244,11 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
 //        ceres_options.max_num_iterations = 1e4;
         ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
-        // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
-        processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
-        sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
+        // SENSOR + PROCESSOR Imu
+        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu.yaml");
+        sen_imu = std::static_pointer_cast<SensorImu>(sen0_ptr);
+        processor_imu = std::static_pointer_cast<ProcessorImu>(processor_);
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -272,7 +272,7 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
         data_imu << -wolf::gravity(), 0,0,0;
         data_imu = data_imu + origin_bias;
 
-        wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov() );//, origin_bias); //set data on IMU (measure only gravity here)
+        wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov() );//, origin_bias); //set data on Imu (measure only gravity here)
 
         for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
         {
@@ -294,15 +294,15 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
     virtual void TearDown(){}
 };
 
-class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
+class FactorImu_biasTest_Static_NonNullBias : public testing::Test
 {
     public:
         wolf::TimeStamp t;
-        SensorIMUPtr sen_imu;
+        SensorImuPtr sen_imu;
         ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
         ProcessorBasePtr processor_;
-        ProcessorIMUPtr processor_imu;
+        ProcessorImuPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
         Eigen::Vector6d origin_bias;
@@ -328,11 +328,11 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
         ceres_options.max_num_iterations = 1e4;
         ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
-        // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
-        processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
-        sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
+        // SENSOR + PROCESSOR Imu
+        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu.yaml");
+        sen_imu = std::static_pointer_cast<SensorImu>(sen0_ptr);
+        processor_imu = std::static_pointer_cast<ProcessorImu>(processor_);
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -358,7 +358,7 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
         data_imu << -wolf::gravity(), 0,0,0;
         data_imu = data_imu + origin_bias;
 
-        wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero()); //set data on IMU (measure only gravity here)
+        wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero()); //set data on Imu (measure only gravity here)
 
         for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
         {
@@ -380,15 +380,15 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
     virtual void TearDown(){}
 };
 
-class FactorIMU_biasTest_Move_NullBias : public testing::Test
+class FactorImu_biasTest_Move_NullBias : public testing::Test
 {
     public:
         wolf::TimeStamp t;
-        SensorIMUPtr sen_imu;
+        SensorImuPtr sen_imu;
         ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
         ProcessorBasePtr processor_;
-        ProcessorIMUPtr processor_imu;
+        ProcessorImuPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
         Eigen::Vector6d origin_bias;
@@ -414,11 +414,11 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test
         ceres_options.max_num_iterations = 1e4;
         ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
-        // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
-        processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
-        sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
+        // SENSOR + PROCESSOR Imu
+        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu.yaml");
+        sen_imu = std::static_pointer_cast<SensorImu>(sen0_ptr);
+        processor_imu = std::static_pointer_cast<ProcessorImu>(processor_);
     
         //===================================================== END{SETTING PROBLEM}
 
@@ -445,7 +445,7 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test
         //===================================================== PROCESS DATA
         // PROCESS DATA
 
-        wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
+        wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
 
         for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
         {
@@ -467,15 +467,15 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test
     virtual void TearDown(){}
 };
 
-class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
+class FactorImu_biasTest_Move_NonNullBias : public testing::Test
 {
     public:
         wolf::TimeStamp t;
-        SensorIMUPtr sen_imu;
+        SensorImuPtr sen_imu;
         ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
         ProcessorBasePtr processor_;
-        ProcessorIMUPtr processor_imu;
+        ProcessorImuPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
         Eigen::Vector6d origin_bias;
@@ -501,11 +501,11 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
         ceres_options.max_num_iterations = 1e4;
         ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
-        // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
-        processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
-        sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
+        // SENSOR + PROCESSOR Imu
+        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu.yaml");
+        sen_imu = std::static_pointer_cast<SensorImu>(sen0_ptr);
+        processor_imu = std::static_pointer_cast<ProcessorImu>(processor_);
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -528,7 +528,7 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
         //===================================================== PROCESS DATA
         // PROCESS DATA
 
-        wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
+        wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
 
         for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
         {
@@ -550,15 +550,15 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
     virtual void TearDown(){}
 };
 
-class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
+class FactorImu_biasTest_Move_NonNullBiasRotCst : public testing::Test
 {
     public:
         wolf::TimeStamp t;
-        SensorIMUPtr sen_imu;
+        SensorImuPtr sen_imu;
         ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
         ProcessorBasePtr processor_;
-        ProcessorIMUPtr processor_imu;
+        ProcessorImuPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
         Eigen::Vector6d origin_bias;
@@ -584,11 +584,11 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
         ceres_options.max_num_iterations = 1e4;
         ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
-        // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
-        processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
-        sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
+        // SENSOR + PROCESSOR Imu
+        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu.yaml");
+        sen_imu = std::static_pointer_cast<SensorImu>(sen0_ptr);
+        processor_imu = std::static_pointer_cast<ProcessorImu>(processor_);
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -617,12 +617,12 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
         //===================================================== END{INITIALIZATION}
         //===================================================== PROCESS DATA
 
-        wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
+        wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
 
         for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
         {
-            //gravity measure depends on current IMU orientation + bias
-            //use data_imu_initial to measure gravity from real orientation of IMU then add biases
+            //gravity measure depends on current Imu orientation + bias
+            //use data_imu_initial to measure gravity from real orientation of Imu then add biases
             data_imu.head(3) = (v2q(data_imu_initial.tail(3) * t.get()).conjugate() * data_imu_initial.head(3)) + origin_bias.head(3);
             t.set(t.get() + 0.001); //increment of 1 ms
             imu_ptr->setData(data_imu);
@@ -643,15 +643,15 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
     virtual void TearDown(){}
 };
 
-class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
+class FactorImu_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
 {
     public:
         wolf::TimeStamp t;
-        SensorIMUPtr sen_imu;
+        SensorImuPtr sen_imu;
         ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
         ProcessorBasePtr processor_;
-        ProcessorIMUPtr processor_imu;
+        ProcessorImuPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
         Eigen::Vector6d origin_bias;
@@ -677,11 +677,11 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
         ceres_options.max_num_iterations = 1e4;
         ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
-        // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
-        processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
-        sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
+        // SENSOR + PROCESSOR Imu
+        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu.yaml");
+        sen_imu = std::static_pointer_cast<SensorImu>(sen0_ptr);
+        processor_imu = std::static_pointer_cast<ProcessorImu>(processor_);
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -716,12 +716,12 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
         //===================================================== PROCESS DATA
         // PROCESS DATA
 
-        wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
+        wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
 
         for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
         {
-            //gravity measure depends on current IMU orientation + bias
-            //use data_imu_initial to measure gravity from real orientation of IMU then add biases
+            //gravity measure depends on current Imu orientation + bias
+            //use data_imu_initial to measure gravity from real orientation of Imu then add biases
             data_imu.head(3) = (v2q(data_imu_initial.tail(3) * t.get()).conjugate() * data_imu_initial.head(3)) + origin_bias.head(3);
             t.set(t.get() + 0.001); //increment of 1 ms
             imu_ptr->setData(data_imu);
@@ -744,15 +744,15 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
 
 // var(b1,b2), inv(p1,q1,v1,p2,q2,v2); fac1: imu(p,q,v)+(b1=b2)
 
-class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
+class FactorImu_biasTest_Move_NonNullBiasRot : public testing::Test
 {
     public:
         wolf::TimeStamp t;
-        SensorIMUPtr sen_imu;
+        SensorImuPtr sen_imu;
         ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
         ProcessorBasePtr processor_;
-        ProcessorIMUPtr processor_imu;
+        ProcessorImuPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
         Eigen::Vector6d origin_bias;
@@ -778,11 +778,11 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
         ceres_options.max_num_iterations = 1e4;
         ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
-        // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
-        processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
-        sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
+        // SENSOR + PROCESSOR Imu
+        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu.yaml");
+        sen_imu = std::static_pointer_cast<SensorImu>(sen0_ptr);
+        processor_imu = std::static_pointer_cast<ProcessorImu>(processor_);
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -806,11 +806,11 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
 
         double dt(0.001);
         TimeStamp ts(0);
-        wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
+        wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
 
         while( ts.get() < 1 )
         {
-            // PROCESS IMU DATA
+            // PROCESS Imu DATA
             // Time and data variables
             ts += dt;
             
@@ -841,22 +841,22 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
     virtual void TearDown(){}
 };
 
-class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
+class FactorImu_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
 {
     public:
         wolf::TimeStamp t;
         ProblemPtr problem;
         CeresManagerPtr ceres_manager;
         SensorBasePtr sensor;
-        SensorIMUPtr sensor_imu;
-        SensorOdom3DPtr sensor_odo;
+        SensorImuPtr sensor_imu;
+        SensorOdom3dPtr sensor_odo;
         ProcessorBasePtr processor;
-        ProcessorIMUPtr processor_imu;
-        ProcessorOdom3DPtr processor_odo;
+        ProcessorImuPtr processor_imu;
+        ProcessorOdom3dPtr processor_odo;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
-        CaptureIMUPtr    capture_imu;
-        CaptureOdom3DPtr capture_odo;
+        CaptureImuPtr    capture_imu;
+        CaptureOdom3dPtr capture_odo;
         Eigen::Vector6d origin_bias;
         Eigen::VectorXd expected_final_state;
         Eigen::VectorXd x_origin;
@@ -878,29 +878,29 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
         ceres_options.minimizer_type = ceres::TRUST_REGION;
         ceres_manager = std::make_shared<CeresManager>(problem, ceres_options);
 
-        // SENSOR + PROCESSOR IMU
-        sensor          = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
-        sensor_imu      = std::static_pointer_cast<SensorIMU>(sensor);
-        processor       = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
-        processor_imu   = std::static_pointer_cast<ProcessorIMU>(processor);
+        // SENSOR + PROCESSOR Imu
+        sensor          = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        sensor_imu      = std::static_pointer_cast<SensorImu>(sensor);
+        processor       = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu.yaml");
+        processor_imu   = std::static_pointer_cast<ProcessorImu>(processor);
 
-        // SENSOR + PROCESSOR ODOM 3D
-        sensor          = problem->installSensor("ODOM 3D", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml");
-        sensor_odo      = std::static_pointer_cast<SensorOdom3D>(sensor);
+        // SENSOR + PROCESSOR ODOM 3d
+        sensor          = problem->installSensor("ODOM 3d", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3d.yaml");
+        sensor_odo      = std::static_pointer_cast<SensorOdom3d>(sensor);
 
         sensor_imu->setNoiseStd((sensor_imu->getNoiseStd()/10.0).eval());
         sensor_odo->setNoiseStd((sensor_odo->getNoiseStd()/10.0).eval());
-        WOLF_TRACE("IMU cov: ", sensor_imu->getNoiseCov().diagonal().transpose());
+        WOLF_TRACE("Imu cov: ", sensor_imu->getNoiseCov().diagonal().transpose());
         WOLF_TRACE("ODO cov: ", sensor_odo->getNoiseCov().diagonal().transpose());
 
-        ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
-        prc_odom3D_params->max_time_span    = 0.0099;
-        prc_odom3D_params->max_buff_length  = 1000; //make it very high so that this condition will not pass
-        prc_odom3D_params->dist_traveled    = 1000;
-        prc_odom3D_params->angle_turned     = 1000;
+        ProcessorParamsOdom3dPtr prc_odom3d_params = std::make_shared<ProcessorParamsOdom3d>();
+        prc_odom3d_params->max_time_span    = 0.0099;
+        prc_odom3d_params->max_buff_length  = 1000; //make it very high so that this condition will not pass
+        prc_odom3d_params->dist_traveled    = 1000;
+        prc_odom3d_params->angle_turned     = 1000;
 
-        processor       = problem->installProcessor("ODOM 3D", "odom", sensor_odo, prc_odom3D_params);
-        processor_odo   = std::static_pointer_cast<ProcessorOdom3D>(processor);
+        processor       = problem->installProcessor("ODOM 3d", "odom", sensor_odo, prc_odom3d_params);
+        processor_odo   = std::static_pointer_cast<ProcessorOdom3d>(processor);
 
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -927,9 +927,9 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
         TimeStamp t_imu(0.0),    t_odo(0.0);
         double   dt_imu(0.001), dt_odo(.01);
 
-        capture_imu = std::make_shared<CaptureIMU>   (t_imu, sensor_imu, data_imu, sensor_imu->getNoiseCov(), sensor_imu->getCalibration(), nullptr);
+        capture_imu = std::make_shared<CaptureImu>   (t_imu, sensor_imu, data_imu, sensor_imu->getNoiseCov(), sensor_imu->getCalibration(), nullptr);
 
-        capture_odo = std::make_shared<CaptureOdom3D>(t_odo, sensor_odo, data_odo, sensor_odo->getNoiseCov(), nullptr);
+        capture_odo = std::make_shared<CaptureOdom3d>(t_odo, sensor_odo, data_odo, sensor_odo->getNoiseCov(), nullptr);
         sensor_odo->process(capture_odo);
         t_odo += dt_odo;        //first odometry data will be processed at this timestamp
 
@@ -943,7 +943,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
         for(unsigned int i = 1; i<=1000; i++)
         {
 
-            // PROCESS IMU DATA
+            // PROCESS Imu DATA
             // Time and data variables
             t_imu += dt_imu;
             
@@ -965,7 +965,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
             WOLF_TRACE("last delta preint: ", processor_imu->getLast()->getDeltaPreint().transpose());
             WOLF_TRACE("last jacoob bias : ", processor_imu->getLast()->getJacobianCalib().row(0));
 
-            //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
+            //when we find a Imu timestamp corresponding with this odometry timestamp then we process odometry measurement
             if(t_imu.get() >= t_odo.get())
             {
                 WOLF_TRACE("====== create ODOM KF ========");
@@ -973,7 +973,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
 //                WOLF_TRACE("last calib: ", processor_imu->getLast()->getCalibration().transpose());
 //                WOLF_TRACE("last calib preint: ", processor_imu->getLast()->getCalibrationPreint().transpose());
 
-                // PROCESS ODOM 3D DATA
+                // PROCESS ODOM 3d DATA
                 data_odo.head(3) << 0,0,0;
                 data_odo.tail(3) = q2v(quat_odo);
                 capture_odo->setTimeStamp(t_odo);
@@ -1025,17 +1025,17 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
     virtual void TearDown(){}
 };
 
-class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
+class FactorImu_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
 {
     public:
         wolf::TimeStamp t;
-        SensorIMUPtr sen_imu;
-        SensorOdom3DPtr sen_odom3D;
+        SensorImuPtr sen_imu;
+        SensorOdom3dPtr sen_odom3d;
         ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
         ProcessorBasePtr processor_;
-        ProcessorIMUPtr processor_imu;
-        ProcessorOdom3DPtr processor_odom3D;
+        ProcessorImuPtr processor_imu;
+        ProcessorOdom3dPtr processor_odom3d;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
         Eigen::Vector6d origin_bias;
@@ -1061,23 +1061,23 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
         ceres_options.max_num_iterations = 1e4;
         ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
-        // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
-        processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
-        sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
-
-        // SENSOR + PROCESSOR ODOM 3D
-        SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3D", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml");
-        ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
-        prc_odom3D_params->max_time_span = 0.9999;
-        prc_odom3D_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
-        prc_odom3D_params->dist_traveled = 1000000000;
-        prc_odom3D_params->angle_turned = 1000000000;
-
-        ProcessorBasePtr processor_odom = problem->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params);
-        sen_odom3D = std::static_pointer_cast<SensorOdom3D>(sen1_ptr);
-        processor_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_odom);
+        // SENSOR + PROCESSOR Imu
+        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu.yaml");
+        sen_imu = std::static_pointer_cast<SensorImu>(sen0_ptr);
+        processor_imu = std::static_pointer_cast<ProcessorImu>(processor_);
+
+        // SENSOR + PROCESSOR ODOM 3d
+        SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3d", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3d.yaml");
+        ProcessorParamsOdom3dPtr prc_odom3d_params = std::make_shared<ProcessorParamsOdom3d>();
+        prc_odom3d_params->max_time_span = 0.9999;
+        prc_odom3d_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
+        prc_odom3d_params->dist_traveled = 1000000000;
+        prc_odom3d_params->angle_turned = 1000000000;
+
+        ProcessorBasePtr processor_odom = problem->installProcessor("ODOM 3d", "odom", sen1_ptr, prc_odom3d_params);
+        sen_odom3d = std::static_pointer_cast<SensorOdom3d>(sen1_ptr);
+        processor_odom3d = std::static_pointer_cast<ProcessorOdom3d>(processor_odom);
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -1093,31 +1093,31 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
 
         //set origin of the problem
         origin_KF = processor_imu->setOrigin(x_origin, t);
-        processor_odom3D->setOrigin(origin_KF);
+        processor_odom3d->setOrigin(origin_KF);
 
         //===================================================== END{INITIALIZATION}
         //===================================================== PROCESS DATA
         // PROCESS DATA
 
-        Eigen::Vector6d data_imu(Eigen::Vector6d::Zero()), data_odom3D(Eigen::Vector6d::Zero());
+        Eigen::Vector6d data_imu(Eigen::Vector6d::Zero()), data_odom3d(Eigen::Vector6d::Zero());
         Eigen::Vector3d rateOfTurn; //deg/s
         rateOfTurn << 0,90,0;
         VectorXd D_cor(10);
 
         double dt(0.0010), dt_odom(1.0);
         TimeStamp ts(0.0), t_odom(0.0);
-        wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
-        wolf::CaptureOdom3DPtr mot_ptr = std::make_shared<CaptureOdom3D>(t, sen_odom3D, data_odom3D, sen_odom3D->getNoiseCov(), nullptr);
-        sen_odom3D->process(mot_ptr);
+        wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
+        wolf::CaptureOdom3dPtr mot_ptr = std::make_shared<CaptureOdom3d>(t, sen_odom3d, data_odom3d, sen_odom3d->getNoiseCov(), nullptr);
+        sen_odom3d->process(mot_ptr);
         //first odometry data will be processed at this timestamp
         t_odom.set(t_odom.get() + dt_odom);
 
         data_imu.tail<3>() = rateOfTurn* M_PI/180.0; //constant rotation =
 
-        //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
+        //when we find a Imu timestamp corresponding with this odometry timestamp then we process odometry measurement
         for(unsigned int i = 1; i<=1000; i++)
         {
-            // PROCESS IMU DATA
+            // PROCESS Imu DATA
             // Time and data variables
             ts.set(i*dt);
             data_imu.head<3>() =  current_quatState.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
@@ -1138,12 +1138,12 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
             {
                 WOLF_TRACE("X_preint(t)  : ", problem->getState(ts).transpose());
 
-                // PROCESS ODOM 3D DATA
-                data_odom3D.head(3) << 0,0,0;
-                data_odom3D.tail(3) = q2v(odom_quat);
+                // PROCESS ODOM 3d DATA
+                data_odom3d.head(3) << 0,0,0;
+                data_odom3d.tail(3) = q2v(odom_quat);
                 mot_ptr->setTimeStamp(t_odom);
-                mot_ptr->setData(data_odom3D);
-                sen_odom3D->process(mot_ptr);
+                mot_ptr->setData(data_odom3d);
+                sen_odom3d->process(mot_ptr);
 
                 //prepare next odometry measurement
                 odom_quat = Eigen::Quaterniond::Identity(); //set to identity to have next odom relative to this last KF
@@ -1166,17 +1166,17 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
     virtual void TearDown(){}
 };
 
-class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
+class FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
 {
     public:
         wolf::TimeStamp t;
-        SensorIMUPtr sen_imu;
-        SensorOdom3DPtr sen_odom3D;
+        SensorImuPtr sen_imu;
+        SensorOdom3dPtr sen_odom3d;
         ProblemPtr problem;
         CeresManagerPtr ceres_manager_wolf_diff;
         ProcessorBasePtr processor;
-        ProcessorIMUPtr processor_imu;
-        ProcessorOdom3DPtr processor_odom3D;
+        ProcessorImuPtr processor_imu;
+        ProcessorOdom3dPtr processor_odom3d;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
         Eigen::Vector6d origin_bias;
@@ -1203,23 +1203,23 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
         ceres_options.max_num_iterations = 1e4;
         ceres_manager_wolf_diff = std::make_shared<CeresManager>(problem, ceres_options);
 
-        // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
-        processor = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
-        sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor);
-
-        // SENSOR + PROCESSOR ODOM 3D
-        SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3D", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml");
-        ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
-        prc_odom3D_params->max_time_span = 0.9999;
-        prc_odom3D_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
-        prc_odom3D_params->dist_traveled = 1000000000;
-        prc_odom3D_params->angle_turned = 1000000000;
-
-        ProcessorBasePtr processor_odom = problem->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params);
-        sen_odom3D = std::static_pointer_cast<SensorOdom3D>(sen1_ptr);
-        processor_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_odom);
+        // SENSOR + PROCESSOR Imu
+        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu.yaml");
+        sen_imu = std::static_pointer_cast<SensorImu>(sen0_ptr);
+        processor_imu = std::static_pointer_cast<ProcessorImu>(processor);
+
+        // SENSOR + PROCESSOR ODOM 3d
+        SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3d", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3d.yaml");
+        ProcessorParamsOdom3dPtr prc_odom3d_params = std::make_shared<ProcessorParamsOdom3d>();
+        prc_odom3d_params->max_time_span = 0.9999;
+        prc_odom3d_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
+        prc_odom3d_params->dist_traveled = 1000000000;
+        prc_odom3d_params->angle_turned = 1000000000;
+
+        ProcessorBasePtr processor_odom = problem->installProcessor("ODOM 3d", "odom", sen1_ptr, prc_odom3d_params);
+        sen_odom3d = std::static_pointer_cast<SensorOdom3d>(sen1_ptr);
+        processor_odom3d = std::static_pointer_cast<ProcessorOdom3d>(processor_odom);
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -1236,31 +1236,31 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
         //set origin of the problem
         origin_KF = problem->setPrior(x_origin, P_origin, t, 0.005);
 //        origin_KF = processor_imu->setOrigin(x_origin, t);
-//        processor_odom3D->setOrigin(origin_KF);
+//        processor_odom3d->setOrigin(origin_KF);
 
         //===================================================== END{INITIALIZATION}
         //===================================================== PROCESS DATA
         // PROCESS DATA
 
-        Eigen::Vector6d data_imu(Eigen::Vector6d::Zero()), data_odom3D(Eigen::Vector6d::Zero());
+        Eigen::Vector6d data_imu(Eigen::Vector6d::Zero()), data_odom3d(Eigen::Vector6d::Zero());
         Eigen::Vector3d rateOfTurn; //deg/s
         rateOfTurn << 45,90,0;
 
         double dt(0.0010), dt_odom(1.0);
         TimeStamp ts(0.0), t_odom(1.0);
-        wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
-        wolf::CaptureOdom3DPtr mot_ptr = std::make_shared<CaptureOdom3D>(t, sen_odom3D, data_odom3D, nullptr);
-        sen_odom3D->process(mot_ptr);
+        wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
+        wolf::CaptureOdom3dPtr mot_ptr = std::make_shared<CaptureOdom3d>(t, sen_odom3d, data_odom3d, nullptr);
+        sen_odom3d->process(mot_ptr);
         //first odometry data will be processed at this timestamp
         //t_odom.set(t_odom.get() + dt_odom);
 
         Eigen::Vector2d randomPart;
         data_imu.tail<3>() = rateOfTurn* M_PI/180.0; //constant rotation =
 
-        //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
+        //when we find a Imu timestamp corresponding with this odometry timestamp then we process odometry measurement
         for(unsigned int i = 1; i<=500; i++)
         {
-            // PROCESS IMU DATA
+            // PROCESS Imu DATA
             // Time and data variables
             ts.set(i*dt);
 
@@ -1278,12 +1278,12 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
 
             if(ts.get() >= t_odom.get())
             {
-                // PROCESS ODOM 3D DATA
-                data_odom3D.head(3) << 0,0,0;
-                data_odom3D.tail(3) = q2v(odom_quat);
+                // PROCESS ODOM 3d DATA
+                data_odom3d.head(3) << 0,0,0;
+                data_odom3d.tail(3) = q2v(odom_quat);
                 mot_ptr->setTimeStamp(t_odom);
-                mot_ptr->setData(data_odom3D);
-                sen_odom3D->process(mot_ptr);
+                mot_ptr->setData(data_odom3d);
+                sen_odom3d->process(mot_ptr);
 
                 //prepare next odometry measurement
                 odom_quat = Eigen::Quaterniond::Identity(); //set to identity to have next odom relative to this last KF
@@ -1296,7 +1296,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
 
         for(unsigned int j = 1; j<=500; j++)
         {
-            // PROCESS IMU DATA
+            // PROCESS Imu DATA
             // Time and data variables
             ts.set((500 + j)*dt);
 
@@ -1317,12 +1317,12 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
 
             if(ts.get() >= t_odom.get())
             {
-                // PROCESS ODOM 3D DATA
-                data_odom3D.head(3) << 0,0,0;
-                data_odom3D.tail(3) = q2v(odom_quat);
+                // PROCESS ODOM 3d DATA
+                data_odom3d.head(3) << 0,0,0;
+                data_odom3d.tail(3) = q2v(odom_quat);
                 mot_ptr->setTimeStamp(t_odom);
-                mot_ptr->setData(data_odom3D);
-                sen_odom3D->process(mot_ptr);
+                mot_ptr->setData(data_odom3d);
+                sen_odom3d->process(mot_ptr);
 
                 //prepare next odometry measurement
                 odom_quat = Eigen::Quaterniond::Identity(); //set to identity to have next odom relative to this last KF
@@ -1345,7 +1345,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
 // tests with following conditions :
 //  var(b1,b2),        invar(p1,q1,v1,p2,q2,v2),    factor : imu(p,q,v)
 
-TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
 {
     //prepare problem for solving
     KF0->getP()->fix();
@@ -1372,7 +1372,7 @@ TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
     ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
 }
 
-TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
 {
     //prepare problem for solving
     KF0->getP()->fix();
@@ -1463,7 +1463,7 @@ TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     }
 }
 
-TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
 {
     //prepare problem for solving
     KF0->getP()->fix();
@@ -1488,7 +1488,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK
     ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
 }
 
-TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
 {
     //prepare problem for solving
     KF0->getP()->fix();
@@ -1579,7 +1579,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia
     }
 }
 
-TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -1601,7 +1601,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_initO
     ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
 }
 
-TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -1681,7 +1681,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
     }
 }
 
-TEST_F(FactorIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorImu_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -1762,7 +1762,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     }
 }
 
-TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorImu_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -1783,7 +1783,7 @@ TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
 
 }
 
-TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorImu_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -1861,7 +1861,7 @@ TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     }
 }
 
-TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorImu_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -1882,7 +1882,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
 
 }
 
-TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorImu_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -1961,7 +1961,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     }
 }
 
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorImu_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -1982,7 +1982,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_initO
 
 }
 
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorImu_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2061,7 +2061,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
     }
 }
 
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2082,7 +2082,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_i
 
 }
 
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2161,7 +2161,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
     }
 }
 
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
+TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2182,7 +2182,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_
 
 }
 
-//TEST_F(FactorIMU_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
+//TEST_F(FactorImu_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
 //{
 //    //prepare problem for solving
 //    origin_KF->getP()->fix();
@@ -2203,7 +2203,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_
 //
 //}
 
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2248,7 +2248,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_i
         WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
 }
 
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2295,7 +2295,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_i
 }
 
 //jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2Q2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2Q2_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2324,7 +2324,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2Q2_i
 }
 
 //jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2354,7 +2354,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_i
 }
 
 //jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2382,7 +2382,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1Q2_i
 }
 
 //jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1Q1_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1Q1_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2411,7 +2411,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1Q1_i
     ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
 }
 
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2461,15 +2461,15 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_i
         WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
 }
 
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_initOK)
 {
     //Add fix factor on yaw to make the problem observable
     Eigen::MatrixXd featureFix_cov(6,6);
     featureFix_cov = Eigen::MatrixXd::Identity(6,6); 
     featureFix_cov(5,5) = 0.1;
-    auto capfix = CaptureBase::emplace<CaptureOdom3D>(origin_KF, 0, nullptr, (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), nullptr);
-    auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3D", (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), featureFix_cov);
-    FactorPose3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(ffix, ffix));
+    auto capfix = CaptureBase::emplace<CaptureOdom3d>(origin_KF, 0, nullptr, (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), nullptr);
+    auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3d", (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), featureFix_cov);
+    FactorPose3dPtr fac_fix = std::static_pointer_cast<FactorPose3d>(FactorBase::emplace<FactorPose3d>(ffix, ffix));
 
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2519,18 +2519,18 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i
         WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
 }
 
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_initOK)
 {
     //Add fix factor on yaw to make the problem observable
     Eigen::MatrixXd featureFix_cov(6,6);
     featureFix_cov = Eigen::MatrixXd::Identity(6,6); 
     featureFix_cov(5,5) = 0.1;
-    // CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureOdom3D>(0, nullptr, (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), nullptr));
-    auto capfix = CaptureBase::emplace<CaptureOdom3D>(origin_KF, 0, nullptr, (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), nullptr);
-    // FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
-    auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3D", (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), featureFix_cov);
-    // FactorPose3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix)));
-    FactorPose3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(ffix, ffix));
+    // CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureOdom3d>(0, nullptr, (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), nullptr));
+    auto capfix = CaptureBase::emplace<CaptureOdom3d>(origin_KF, 0, nullptr, (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), nullptr);
+    // FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3d", (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
+    auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3d", (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), featureFix_cov);
+    // FactorPose3dPtr fac_fix = std::static_pointer_cast<FactorPose3d>(ffix->addFactor(std::make_shared<FactorPose3d>(ffix)));
+    FactorPose3dPtr fac_fix = std::static_pointer_cast<FactorPose3d>(FactorBase::emplace<FactorPose3d>(ffix, ffix));
 
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2580,7 +2580,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_
         WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
 }
 
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2601,7 +2601,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V2_in
 //    WOLF_TRACE("real   bias : ", origin_bias.transpose());
 //    WOLF_TRACE("origin bias : ", origin_KF->getCaptureOf(sensor_imu)->getCalibration().transpose());
 //    WOLF_TRACE("last   bias : ", last_KF->getCaptureOf(sensor_imu)->getCalibration().transpose());
-//    WOLF_TRACE("jacob  bias : ", std::static_pointer_cast<CaptureIMU>(last_KF->getCaptureOf(sensor_imu))->getJacobianCalib().row(0));
+//    WOLF_TRACE("jacob  bias : ", std::static_pointer_cast<CaptureImu>(last_KF->getCaptureOf(sensor_imu))->getJacobianCalib().row(0));
 
     std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
     std::cout << report << std::endl;
@@ -2631,7 +2631,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V2_in
         WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
 }
 
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q2_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2677,7 +2677,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q2_in
         WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
 }
 
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q2_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2704,7 +2704,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q2_in
     ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
 }
 
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P2_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2733,7 +2733,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P2_in
     ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
 }
 
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2760,7 +2760,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_in
     ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
 }
 
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q1_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q1_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2789,7 +2789,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q1_in
     ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
 }
 
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
 {
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2844,10 +2844,10 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_in
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
-//  ::testing::GTEST_FLAG(filter) = "FactorIMU_biasTest_Move_NonNullBiasRot.*:FactorIMU_biasTest_Static_NullBias.*:FactorIMU_biasTest_Static_NonNullAccBias.*:FactorIMU_biasTest_Static_NonNullGyroBias.*";
-//  ::testing::GTEST_FLAG(filter) = "FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY.VarB1B2V1V2_InvarP1Q1P2Q2_initOK";
-//  ::testing::GTEST_FLAG(filter) = "FactorIMU_ODOM_biasTest_Move_NonNullBiasRot.VarB1B2_InvarP1Q1V1P2Q2V2_initOK";
-//  ::testing::GTEST_FLAG(filter) = "FactorIMU_biasTest_Move_NonNullBiasRot.VarB1B2V1P2V2_InvarP1Q1Q2_initOK";
+//  ::testing::GTEST_FLAG(filter) = "FactorImu_biasTest_Move_NonNullBiasRot.*:FactorImu_biasTest_Static_NullBias.*:FactorImu_biasTest_Static_NonNullAccBias.*:FactorImu_biasTest_Static_NonNullGyroBias.*";
+//  ::testing::GTEST_FLAG(filter) = "FactorImu_ODOM_biasTest_Move_NonNullBiasRotY.VarB1B2V1V2_InvarP1Q1P2Q2_initOK";
+//  ::testing::GTEST_FLAG(filter) = "FactorImu_ODOM_biasTest_Move_NonNullBiasRot.VarB1B2_InvarP1Q1V1P2Q2V2_initOK";
+//  ::testing::GTEST_FLAG(filter) = "FactorImu_biasTest_Move_NonNullBiasRot.VarB1B2V1P2V2_InvarP1Q1Q2_initOK";
 
   return RUN_ALL_TESTS();
 }
diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_imu.cpp
similarity index 83%
rename from test/gtest_feature_IMU.cpp
rename to test/gtest_feature_imu.cpp
index ba537cc567b516cbc4ee5470644b2d059296e372..6e801064b2d91fccd011a9637a01ee15b6185856 100644
--- a/test/gtest_feature_IMU.cpp
+++ b/test/gtest_feature_imu.cpp
@@ -1,7 +1,7 @@
 //Wolf
-#include "IMU/capture/capture_IMU.h"
-#include "IMU/processor/processor_IMU.h"
-#include "IMU/sensor/sensor_IMU.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/processor/processor_imu.h"
+#include "imu/sensor/sensor_imu.h"
 #include "core/common/wolf.h"
 #include "core/problem/problem.h"
 #include "core/state_block/state_block.h"
@@ -9,17 +9,17 @@
 #include <core/utils/utils_gtest.h>
 #include "core/utils/logging.h"
 
-class FeatureIMU_test : public testing::Test
+class FeatureImu_test : public testing::Test
 {
 
     public: //These can be accessed in fixtures
         wolf::ProblemPtr problem;
         wolf::TimeStamp ts;
-        wolf::CaptureIMUPtr imu_ptr;
+        wolf::CaptureImuPtr imu_ptr;
         Eigen::VectorXd state_vec;
         Eigen::VectorXd delta_preint;
         Eigen::Matrix<double,9,9> delta_preint_cov;
-        std::shared_ptr<wolf::FeatureIMU> feat_imu;
+        std::shared_ptr<wolf::FeatureImu> feat_imu;
         wolf::FrameBasePtr last_frame;
         wolf::FrameBasePtr origin_frame;
         Eigen::MatrixXd dD_db_jacobians;
@@ -36,16 +36,16 @@ class FeatureIMU_test : public testing::Test
 
         // Wolf problem
         problem = Problem::create("POV", 3);
-        Eigen::VectorXd IMU_extrinsics(7);
-        IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
-        ParamsSensorIMUPtr sen_imu_params = std::make_shared<ParamsSensorIMU>();
-        SensorBasePtr sensor_ptr = problem->installSensor("SensorIMU", "Main IMU", IMU_extrinsics, sen_imu_params);
-        ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
+        Eigen::VectorXd Imu_extrinsics(7);
+        Imu_extrinsics << 0,0,0, 0,0,0,1; // Imu pose in the robot
+        ParamsSensorImuPtr sen_imu_params = std::make_shared<ParamsSensorImu>();
+        SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", "Main Imu", Imu_extrinsics, sen_imu_params);
+        ProcessorParamsImuPtr prc_imu_params = std::make_shared<ProcessorParamsImu>();
         prc_imu_params->max_time_span   = 0.5;
         prc_imu_params->max_buff_length = 10;
         prc_imu_params->dist_traveled   = 5;
         prc_imu_params->angle_turned    = 0.5;
-        processor_ptr_ = problem->installProcessor("ProcessorIMU", "IMU pre-integrator", sensor_ptr, prc_imu_params);
+        processor_ptr_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_ptr, prc_imu_params);
         processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(processor_ptr_);
 
     // Time and data variables
@@ -60,10 +60,10 @@ class FeatureIMU_test : public testing::Test
         MatrixXd P0; P0.setIdentity(9,9);
         origin_frame = problem->setPrior(x0, P0, 0.0, 0.01);
 
-    // Emplace one capture to store the IMU data arriving from (sensor / callback / file / etc.)
+    // Emplace one capture to store the Imu data arriving from (sensor / callback / file / etc.)
     // give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
-        imu_ptr = std::static_pointer_cast<CaptureIMU>(
-                CaptureBase::emplace<CaptureIMU>(origin_frame,
+        imu_ptr = std::static_pointer_cast<CaptureImu>(
+                CaptureBase::emplace<CaptureImu>(origin_frame,
                                                  t,
                                                  sensor_ptr,
                                                  data_,
@@ -90,8 +90,8 @@ class FeatureIMU_test : public testing::Test
         delta_preint_cov        = processor_motion_ptr_->getMotion().delta_integr_cov_ + MatrixXd::Identity(9,9)*1e-08;
         VectorXd calib_preint   = processor_motion_ptr_->getLast()->getCalibrationPreint();
         dD_db_jacobians         = processor_motion_ptr_->getMotion().jacobian_calib_;
-        feat_imu                = std::static_pointer_cast<FeatureIMU>(
-                FeatureBase::emplace<FeatureIMU>(imu_ptr,
+        feat_imu                = std::static_pointer_cast<FeatureImu>(
+                FeatureBase::emplace<FeatureImu>(imu_ptr,
                                                  delta_preint,
                                                  delta_preint_cov,
                                                  calib_preint,
@@ -113,7 +113,7 @@ class FeatureIMU_test : public testing::Test
     }
 };
 
-TEST_F(FeatureIMU_test, check_frame)
+TEST_F(FeatureImu_test, check_frame)
 {
     // set variables
     using namespace wolf;
@@ -144,7 +144,7 @@ TEST_F(FeatureIMU_test, check_frame)
     ASSERT_EQ(origin_frame->id(), left_frame->id());
 }
 
-TEST_F(FeatureIMU_test, access_members)
+TEST_F(FeatureImu_test, access_members)
 {
     using namespace wolf;
 
@@ -154,13 +154,13 @@ TEST_F(FeatureIMU_test, access_members)
     ASSERT_MATRIX_APPROX(feat_imu->getMeasurement().transpose(), delta.transpose(), wolf::Constants::EPS);
 }
 
-//TEST_F(FeatureIMU_test, addFactor)
+//TEST_F(FeatureImu_test, addFactor)
 //{
 //    using namespace wolf;
 //
 //    FrameBasePtr frm_imu = std::static_pointer_cast<FrameBase>(last_frame);
 //    auto cap_imu = last_frame->getCaptureList().back();
-//    FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, std::static_pointer_cast<CaptureIMU>(cap_imu), processor_ptr_);
+//    FactorImuPtr factor_imu = std::make_shared<FactorImu>(feat_imu, std::static_pointer_cast<CaptureImu>(cap_imu), processor_ptr_);
 //    feat_imu->addFactor(factor_imu);
 //    origin_frame->addConstrainedBy(factor_imu);
 //}
diff --git a/test/gtest_IMU.cpp b/test/gtest_imu.cpp
similarity index 92%
rename from test/gtest_IMU.cpp
rename to test/gtest_imu.cpp
index 52aff0fcd243ed30cd602af4e4f801a0e7347e42..37bef1d89728b9590f9db3b3b4574d1d60889fd7 100644
--- a/test/gtest_IMU.cpp
+++ b/test/gtest_imu.cpp
@@ -1,19 +1,19 @@
 /*
- * gtest_IMU.cpp
+ * gtest_Imu.cpp
  *
  *  Created on: Nov 14, 2017
  *      Author: jsola
  */
 
 //Wolf
-#include "IMU/processor/processor_IMU.h"
-#include "IMU/sensor/sensor_IMU.h"
+#include "imu/processor/processor_imu.h"
+#include "imu/sensor/sensor_imu.h"
 
-#include "IMU/internal/config.h"
+#include "imu/internal/config.h"
 
 #include <core/common/wolf.h>
-#include <core/sensor/sensor_odom_3D.h>
-#include <core/processor/processor_odom_3D.h>
+#include <core/sensor/sensor_odom_3d.h>
+#include <core/processor/processor_odom_3d.h>
 #include <core/ceres_wrapper/ceres_manager.h>
 
 #include <core/utils/utils_gtest.h>
@@ -27,15 +27,15 @@ using std::make_shared;
 using std::static_pointer_cast;
 using std::string;
 
-class Process_Factor_IMU : public testing::Test
+class Process_Factor_Imu : public testing::Test
 {
     public:
         // Wolf objects
         ProblemPtr          problem;
         CeresManagerPtr     ceres_manager;
-        SensorIMUPtr        sensor_imu;
-        ProcessorIMUPtr     processor_imu;
-        CaptureIMUPtr       capture_imu;
+        SensorImuPtr        sensor_imu;
+        ProcessorImuPtr     processor_imu;
+        CaptureImuPtr       capture_imu;
         FrameBasePtr        KF_0, KF_1;     // keyframes
         CaptureBasePtr      C_0 , C_1;      // base captures
         CaptureMotionPtr    CM_0, CM_1;     // motion captures
@@ -56,9 +56,9 @@ class Process_Factor_IMU : public testing::Test
         Vector6d            bias_0, bias_1;                     // optimized bias at KF's 0 and 1
 
         // input
-        Matrix<double, 6, Dynamic> motion;                      // Motion in IMU frame. Each column is a motion step. If just one column, then the number of steps is defined in num_integrations
-        Matrix<double, 3, Dynamic> a, w;                        // True acc and angvel in IMU frame. Each column is a motion step. Used to create motion with `motion << a,w;`
-        Vector6d            data;                               // IMU data. It's the motion with gravity and bias. See imu::motion2data().
+        Matrix<double, 6, Dynamic> motion;                      // Motion in Imu frame. Each column is a motion step. If just one column, then the number of steps is defined in num_integrations
+        Matrix<double, 3, Dynamic> a, w;                        // True acc and angvel in Imu frame. Each column is a motion step. Used to create motion with `motion << a,w;`
+        Vector6d            data;                               // Imu data. It's the motion with gravity and bias. See imu::motion2data().
 
         // Deltas and states (exact, integrated, corrected, solved, etc)
         VectorXd        D_exact,         x1_exact;          // exact or ground truth
@@ -96,11 +96,11 @@ class Process_Factor_IMU : public testing::Test
             ceres::Solver::Options ceres_options;
             ceres_manager = make_shared<CeresManager>(problem, ceres_options);
 
-            // SENSOR + PROCESSOR IMU
-            SensorBasePtr       sensor = problem->installSensor   ("SensorIMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
-            ProcessorBasePtr processor = problem->installProcessor("ProcessorIMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
-            sensor_imu    = static_pointer_cast<SensorIMU>   (sensor);
-            processor_imu = static_pointer_cast<ProcessorIMU>(processor);
+            // SENSOR + PROCESSOR Imu
+            SensorBasePtr       sensor = problem->installSensor   ("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+            ProcessorBasePtr processor = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu.yaml");
+            sensor_imu    = static_pointer_cast<SensorImu>   (sensor);
+            processor_imu = static_pointer_cast<ProcessorImu>(processor);
 
             dt = 0.01;
             processor_imu->setTimeTolerance(dt/2);
@@ -120,7 +120,7 @@ class Process_Factor_IMU : public testing::Test
          * Input:
          *   q: current orientation
          *   motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
-         *   bias_real: the real bias of the IMU
+         *   bias_real: the real bias of the Imu
          *   bias_preint: the bias used for Delta pre-integration
          * Input/output
          *   Delta: the preintegrated delta
@@ -158,7 +158,7 @@ class Process_Factor_IMU : public testing::Test
          *   N: number of steps
          *   q0: initial orientation
          *   motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
-         *   bias_real: the real bias of the IMU
+         *   bias_real: the real bias of the Imu
          *   bias_preint: the bias used for Delta pre-integration
          * Output:
          *   return: the preintegrated delta
@@ -180,7 +180,7 @@ class Process_Factor_IMU : public testing::Test
          *   N: number of steps
          *   q0: initial orientation
          *   motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
-         *   bias_real: the real bias of the IMU
+         *   bias_real: the real bias of the Imu
          *   bias_preint: the bias used for Delta pre-integration
          * Output:
          *   J_D_b: the Jacobian of the preintegrated delta wrt the bias
@@ -205,7 +205,7 @@ class Process_Factor_IMU : public testing::Test
          * Input:
          *   q0: initial orientation
          *   motion: Matrix with N columns [ax, ay, az, wx, wy, wz] with the true magnitudes in body frame
-         *   bias_real: the real bias of the IMU
+         *   bias_real: the real bias of the Imu
          *   bias_preint: the bias used for Delta pre-integration
          * Output:
          *   J_D_b: the Jacobian of the preintegrated delta wrt the bias
@@ -230,7 +230,7 @@ class Process_Factor_IMU : public testing::Test
          * Input:
          *   q0: initial orientation
          *   motion: Matrix with N columns [ax, ay, az, wx, wy, wz] with the true magnitudes in body frame
-         *   bias_real: the real bias of the IMU
+         *   bias_real: the real bias of the Imu
          *   bias_preint: the bias used for Delta pre-integration
          * Output:
          *   J_D_b: the Jacobian of the preintegrated delta wrt the bias
@@ -261,7 +261,7 @@ class Process_Factor_IMU : public testing::Test
         {
             Vector6d      motion_now;
             data        = imu::motion2data(motion.col(0), q0, bias_real);
-            capture_imu = make_shared<CaptureIMU>(t0, sensor_imu, data, sensor_imu->getNoiseCov());
+            capture_imu = make_shared<CaptureImu>(t0, sensor_imu, data, sensor_imu->getNoiseCov());
             q           = q0;
             t           = t0;
             for (int i= 0; i < N; i++)
@@ -324,14 +324,14 @@ class Process_Factor_IMU : public testing::Test
         // Integrate using all methods
         virtual void integrateAll()
         {
-            // ===================================== INTEGRATE EXACTLY WITH IMU_TOOLS with no bias at all
+            // ===================================== INTEGRATE EXACTLY WITH Imu_TOOLS with no bias at all
             if (motion.cols() == 1)
                 D_exact = integrateDelta(num_integrations, q0, motion, bias_null, bias_null, dt);
             else
                 D_exact = integrateDelta(q0, motion, bias_null, bias_null, dt, J_D_bias);
             x1_exact = imu::composeOverState(x0, D_exact, DT );
 
-            // ===================================== INTEGRATE USING IMU_TOOLS
+            // ===================================== INTEGRATE USING Imu_TOOLS
             // pre-integrate
             if (motion.cols() == 1)
                 D_preint_imu = integrateDelta(num_integrations, q0, motion, bias_real, bias_preint, dt, J_D_bias);
@@ -346,7 +346,7 @@ class Process_Factor_IMU : public testing::Test
             x1_preint_imu    = imu::composeOverState(x0, D_preint_imu    , DT );
             x1_corrected_imu = imu::composeOverState(x0, D_corrected_imu , DT );
 
-            // ===================================== INTEGRATE USING PROCESSOR_IMU
+            // ===================================== INTEGRATE USING PROCESSOR_Imu
 
             integrateWithProcessor(num_integrations, t0, q0, motion, bias_real, bias_preint, dt, D_preint, D_corrected);
 
@@ -362,7 +362,7 @@ class Process_Factor_IMU : public testing::Test
             Trj_D_exact.resize(10,cols); Trj_D_preint_imu.resize(10,cols); Trj_D_preint_prc.resize(10,cols); Trj_D_corrected_imu.resize(10,cols); Trj_D_corrected_prc.resize(10,cols);
             Trj_x_exact.resize(10,cols); Trj_x_preint_imu.resize(10,cols); Trj_x_preint_prc.resize(10,cols); Trj_x_corrected_imu.resize(10,cols); Trj_x_corrected_prc.resize(10,cols);
 
-            // ===================================== INTEGRATE EXACTLY WITH IMU_TOOLS with no bias at all
+            // ===================================== INTEGRATE EXACTLY WITH Imu_TOOLS with no bias at all
             MotionBuffer Buf_exact = integrateDeltaTrajectory(q0, motion, bias_null, bias_null, dt, J_D_bias);
 
             // Build exact trajectories
@@ -380,7 +380,7 @@ class Process_Factor_IMU : public testing::Test
             D_exact          = Trj_D_exact.col(cols-1);
             x1_exact         = Trj_x_exact.col(cols-1);
 
-            // ===================================== INTEGRATE USING IMU_TOOLS
+            // ===================================== INTEGRATE USING Imu_TOOLS
             // pre-integrate
             MotionBuffer Buf_preint_imu = integrateDeltaTrajectory(q0, motion, bias_real, bias_preint, dt, J_D_bias);
 
@@ -411,11 +411,11 @@ class Process_Factor_IMU : public testing::Test
             x1_preint_imu    = imu::composeOverState(x0, D_preint_imu    , DT );
             x1_corrected_imu = imu::composeOverState(x0, D_corrected_imu , DT );
 
-            // ===================================== INTEGRATE USING PROCESSOR_IMU
+            // ===================================== INTEGRATE USING PROCESSOR_Imu
 
             MotionBuffer Buf_D_preint_prc = integrateWithProcessor(num_integrations, t0, q0, motion, bias_real, bias_preint, dt, D_preint, D_corrected);
 
-            // Build trajectories preintegrated and corrected with processorIMU
+            // Build trajectories preintegrated and corrected with processorImu
             Dt = 0;
             col = 0;
             Buf_Jac_preint_prc.clear();
@@ -485,16 +485,16 @@ class Process_Factor_IMU : public testing::Test
             // ===================================== SET KF in Wolf tree
             FrameBasePtr KF = problem->emplaceFrame(KEY, x1_exact, t);
 
-            // ===================================== IMU CALLBACK
+            // ===================================== Imu CALLBACK
             problem->keyFrameCallback(KF, nullptr, dt/2);
 
-            // Process IMU for the callback to take effect
+            // Process Imu for the callback to take effect
             data = Vector6d::Zero();
-            capture_imu = make_shared<CaptureIMU>(t+dt, sensor_imu, data, sensor_imu->getNoiseCov());
+            capture_imu = make_shared<CaptureImu>(t+dt, sensor_imu, data, sensor_imu->getNoiseCov());
             sensor_imu->process(capture_imu);
 
             KF_1 = problem->getLastKeyFrame();
-            C_1  = KF_1->getCaptureList().front(); // front is IMU
+            C_1  = KF_1->getCaptureList().front(); // front is Imu
             CM_1 = static_pointer_cast<CaptureMotion>(C_1);
 
             // ===================================== SET BOUNDARY CONDITIONS
@@ -586,37 +586,37 @@ class Process_Factor_IMU : public testing::Test
 
 };
 
-class Process_Factor_IMU_ODO : public Process_Factor_IMU
+class Process_Factor_Imu_ODO : public Process_Factor_Imu
 {
     public:
         // Wolf objects
-        SensorOdom3DPtr     sensor_odo;
-        ProcessorOdom3DPtr  processor_odo;
-        CaptureOdom3DPtr    capture_odo;
+        SensorOdom3dPtr     sensor_odo;
+        ProcessorOdom3dPtr  processor_odo;
+        CaptureOdom3dPtr    capture_odo;
 
         virtual void SetUp( ) override
         {
 
-            // ===================================== IMU
-            Process_Factor_IMU::SetUp();
+            // ===================================== Imu
+            Process_Factor_Imu::SetUp();
 
             // ===================================== ODO
             string wolf_root = _WOLF_IMU_ROOT_DIR;
 
-            SensorBasePtr    sensor     = problem->installSensor   ("SensorOdom3D", "Odometer", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml"   );
-            ProcessorBasePtr processor  = problem->installProcessor("ProcessorOdom3D", "Odometer", "Odometer"                            , wolf_root + "/demos/processor_odom_3D.yaml");
+            SensorBasePtr    sensor     = problem->installSensor   ("SensorOdom3d", "Odometer", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3d.yaml"   );
+            ProcessorBasePtr processor  = problem->installProcessor("ProcessorOdom3d", "Odometer", "Odometer"                            , wolf_root + "/demos/processor_odom_3d.yaml");
 
-            sensor_odo      = static_pointer_cast<SensorOdom3D>(sensor);
+            sensor_odo      = static_pointer_cast<SensorOdom3d>(sensor);
 
-            processor_odo   = static_pointer_cast<ProcessorOdom3D>(processor);
+            processor_odo   = static_pointer_cast<ProcessorOdom3d>(processor);
             processor_odo->setTimeTolerance(dt/2);
             processor_odo->setVotingActive(false);
         }
 
         virtual void integrateAll() override
         {
-            // ===================================== IMU
-            Process_Factor_IMU::integrateAll();
+            // ===================================== Imu
+            Process_Factor_Imu::integrateAll();
 
             // ===================================== ODO
             Vector6d    data;
@@ -626,15 +626,15 @@ class Process_Factor_IMU_ODO : public Process_Factor_IMU
             Vector3d    dth = log_q( q0.conjugate() *  q1     );
             data           << dp, dth;
 
-            CaptureOdom3DPtr capture_odo = make_shared<CaptureOdom3D>(t, sensor_odo, data, sensor_odo->getNoiseCov());
+            CaptureOdom3dPtr capture_odo = make_shared<CaptureOdom3d>(t, sensor_odo, data, sensor_odo->getNoiseCov());
 
             sensor_odo->process(capture_odo);
        }
 
         virtual void integrateAllTrajectories() override
         {
-            // ===================================== IMU
-            Process_Factor_IMU::integrateAllTrajectories();
+            // ===================================== Imu
+            Process_Factor_Imu::integrateAllTrajectories();
 
             // ===================================== ODO
             Vector6d    data;
@@ -644,27 +644,27 @@ class Process_Factor_IMU_ODO : public Process_Factor_IMU
             Vector3d    dth = log_q( q0.conjugate() *  q1     );
             data           << dp, dth;
 
-            CaptureOdom3DPtr capture_odo = make_shared<CaptureOdom3D>(t, sensor_odo, data, sensor_odo->getNoiseCov());
+            CaptureOdom3dPtr capture_odo = make_shared<CaptureOdom3d>(t, sensor_odo, data, sensor_odo->getNoiseCov());
 
             sensor_odo->process(capture_odo);
        }
 
         virtual void buildProblem() override
         {
-            // ===================================== IMU
-            Process_Factor_IMU::buildProblem();
+            // ===================================== Imu
+            Process_Factor_Imu::buildProblem();
 
             // ===================================== ODO
             // Process ODO for the callback to take effect
             data = Vector6d::Zero();
-            capture_odo = make_shared<CaptureOdom3D>(t+dt, sensor_odo, data, sensor_odo->getNoiseCov());
+            capture_odo = make_shared<CaptureOdom3d>(t+dt, sensor_odo, data, sensor_odo->getNoiseCov());
             sensor_odo->process(capture_odo);
 
         }
 
 };
 
-TEST_F(Process_Factor_IMU, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimated
 {
 
     // ================================================================================================================ //
@@ -708,7 +708,7 @@ TEST_F(Process_Factor_IMU, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimated
 
 }
 
-TEST_F(Process_Factor_IMU, test_capture) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, test_capture) // F_ixed___e_stimated
 {
 
     // ================================================================================================================ //
@@ -754,7 +754,7 @@ TEST_F(Process_Factor_IMU, test_capture) // F_ixed___e_stimated
     ASSERT_MATRIX_APPROX(KF_0->getCaptureOf(sensor_imu)->getCalibration(), KF_1->getCaptureOf(sensor_imu)->getCalibration(), 1e-8 );
 }
 
-TEST_F(Process_Factor_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimated
 {
 
     // ================================================================================================================ //
@@ -798,7 +798,7 @@ TEST_F(Process_Factor_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimated
 
 }
 
-TEST_F(Process_Factor_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimated
 {
 
     // ================================================================================================================ //
@@ -842,7 +842,7 @@ TEST_F(Process_Factor_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimated
 
 }
 
-TEST_F(Process_Factor_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
 {
 
     // ================================================================================================================ //
@@ -886,7 +886,7 @@ TEST_F(Process_Factor_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
 
 }
 
-TEST_F(Process_Factor_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
 {
 
     // ================================================================================================================ //
@@ -930,7 +930,7 @@ TEST_F(Process_Factor_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
 
 }
 
-TEST_F(Process_Factor_IMU, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
 {
 
     // ================================================================================================================ //
@@ -974,7 +974,7 @@ TEST_F(Process_Factor_IMU, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
 
 }
 
-TEST_F(Process_Factor_IMU, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ixed___e_stimated
 {
 
     // ================================================================================================================ //
@@ -1018,7 +1018,7 @@ TEST_F(Process_Factor_IMU, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ixed__
 
 }
 
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixed___e_stimated
 {
 
     // ================================================================================================================ //
@@ -1062,7 +1062,7 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixed___
 
 }
 
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixed___e_stimated
 {
 
     // ================================================================================================================ //
@@ -1106,7 +1106,7 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixed___
 
 }
 
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixed___e_stimated
 {
 
     // ================================================================================================================ //
@@ -1150,7 +1150,7 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixed___
 
 }
 
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixed___e_stimated
 {
 
     // ================================================================================================================ //
@@ -1194,7 +1194,7 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixed___
 
 }
 
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixed___e_stimated
 {
 
     // ================================================================================================================ //
@@ -1238,7 +1238,7 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixed___
 
 }
 
-TEST_F(Process_Factor_IMU_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_stimated
 {
 
     // ================================================================================================================ //
@@ -1282,7 +1282,7 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_stimat
 
 }
 
-TEST_F(Process_Factor_IMU_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_stimated
 {
 
     // ================================================================================================================ //
@@ -1326,7 +1326,7 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_stimat
 
 }
 
-TEST_F(Process_Factor_IMU_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stimated
 {
 
     // ================================================================================================================ //
@@ -1370,7 +1370,7 @@ TEST_F(Process_Factor_IMU_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stimated
 
 }
 
-TEST_F(Process_Factor_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
 {
 
     // ================================================================================================================ //
@@ -1414,7 +1414,7 @@ TEST_F(Process_Factor_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
 
 }
 
-TEST_F(Process_Factor_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
 {
 
     // ================================================================================================================ //
@@ -1502,28 +1502,28 @@ TEST_F(Process_Factor_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);
-    //    ::testing::GTEST_FLAG(filter) = "Process_Factor_IMU.*";
-    //    ::testing::GTEST_FLAG(filter) = "Process_Factor_IMU_ODO.*";
-    //    ::testing::GTEST_FLAG(filter) = "Process_Factor_IMU_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b";
+    //    ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu.*";
+    //    ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.*";
+    //    ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b";
 
     return RUN_ALL_TESTS();
 }
 
 /* Some notes :
  *
- * - Process_Factor_IMU_ODO.MotionConstant_PQv_b__PQv_b :
+ * - Process_Factor_Imu_ODO.MotionConstant_PQv_b__PQv_b :
  *      this test will not work + jacobian is rank deficient because of estimating both initial
  *      and final velocities. 
- *      IMU data integration is done with correct biases (so this is the case of a calibrated IMU). Before solving the problem, we perturbate the initial bias.
+ *      Imu data integration is done with correct biases (so this is the case of a calibrated Imu). Before solving the problem, we perturbate the initial bias.
  *      We solve the problem by fixing all states excepted V1 and V2. while creating the factors, both velocities are corrected using the difference between the actual
  *      bias and the bias used during preintegration. One way to solve this in the solver side would be to make the actual bias match the preintegraion bias so that the
  *      difference is 0 and does not affect the states of the KF. Another possibility is to have both velocities modified without changing the biases. it is likely that this
  *      solution is chosen in this case (bias changes is penalized between 2 KeyFrames, but velocities have no other factors here.)
  * 
  *  - Bias evaluation with a precision of 1e-4 :
- *      The bias introduced in the data for the preintegration steps is different of the real bias. This is simulating the case of a non calibrated IMU
+ *      The bias introduced in the data for the preintegration steps is different of the real bias. This is simulating the case of a non calibrated Imu
  *      Because of cross relations between acc and gyro biases (respectively a_b and w_b) it is difficult to expect a better estimation.
  *      A small change in a_b can be cancelled by a small variation in w_b. in other words : there are local minima.
- *      In addition, for Process_Factor_IMU tests, P and V are tested against 1e-5 precision while 1e-8 is used for Q.
+ *      In addition, for Process_Factor_Imu tests, P and V are tested against 1e-5 precision while 1e-8 is used for Q.
  *      Errors tend to be distributed in different estimated variable when we get into a local minima (to minimize residuals in a least square sense).
  */
diff --git a/test/gtest_IMU_tools.cpp b/test/gtest_imu_tools.cpp
similarity index 95%
rename from test/gtest_IMU_tools.cpp
rename to test/gtest_imu_tools.cpp
index 16730096e0e31900010b18a13ca11c1ffa21a20a..c2e8306ce5b7d0c6cacafc1ac30f8be0474cf0e9 100644
--- a/test/gtest_IMU_tools.cpp
+++ b/test/gtest_imu_tools.cpp
@@ -5,14 +5,14 @@
  *      Author: jsola
  */
 
-#include "IMU/math/IMU_tools.h"
+#include "imu/math/imu_tools.h"
 #include <core/utils/utils_gtest.h>
 
 using namespace Eigen;
 using namespace wolf;
 using namespace imu;
 
-TEST(IMU_tools, identity)
+TEST(Imu_tools, identity)
 {
     VectorXd id_true;
     id_true.setZero(10);
@@ -22,7 +22,7 @@ TEST(IMU_tools, identity)
     ASSERT_MATRIX_APPROX(id, id_true, 1e-10);
 }
 
-TEST(IMU_tools, identity_split)
+TEST(Imu_tools, identity_split)
 {
     VectorXd p(3), qv(4), v(3);
     Quaterniond q;
@@ -38,7 +38,7 @@ TEST(IMU_tools, identity_split)
     ASSERT_MATRIX_APPROX(v , Vector3d::Zero(), 1e-10);
 }
 
-TEST(IMU_tools, inverse)
+TEST(Imu_tools, inverse)
 {
     VectorXd d(10), id(10), iid(10), iiid(10), I(10);
     Vector4d qv;
@@ -60,7 +60,7 @@ TEST(IMU_tools, inverse)
     ASSERT_MATRIX_APPROX(id, iiid, 1e-10);
 }
 
-TEST(IMU_tools, compose_between)
+TEST(Imu_tools, compose_between)
 {
     VectorXd dx1(10), dx2(10), dx3(10);
     Matrix<double, 10, 1> d1, d2, d3;
@@ -94,7 +94,7 @@ TEST(IMU_tools, compose_between)
     ASSERT_MATRIX_APPROX(diff, inverse(d2, dt), 1e-10);
 }
 
-TEST(IMU_tools, compose_between_with_state)
+TEST(Imu_tools, compose_between_with_state)
 {
     VectorXd x(10), d(10), x2(10), x3(10), d2(10), d3(10);
     Vector4d qv;
@@ -123,12 +123,12 @@ TEST(IMU_tools, compose_between_with_state)
     ASSERT_MATRIX_APPROX(betweenStates(x, composeOverState(x, d, dt), dt), d, 1e-10);
 }
 
-TEST(IMU_tools, lift_retract)
+TEST(Imu_tools, lift_retract)
 {
     VectorXd d_min(9); d_min << 0, 1, 2, .3, .4, .5, 6, 7, 8; // use angles in the ball theta < pi
 
     // transform to delta
-    VectorXd delta = exp_IMU(d_min);
+    VectorXd delta = exp_Imu(d_min);
 
     // expected delta
     Vector3d dp = d_min.head(3);
@@ -138,15 +138,15 @@ TEST(IMU_tools, lift_retract)
     ASSERT_MATRIX_APPROX(delta, delta_true, 1e-10);
 
     // transform to a new tangent -- should be the original tangent
-    VectorXd d_from_delta = log_IMU(delta);
+    VectorXd d_from_delta = log_Imu(delta);
     ASSERT_MATRIX_APPROX(d_from_delta, d_min, 1e-10);
 
     // transform to a new delta -- should be the previous delta
-    VectorXd delta_from_d = exp_IMU(d_from_delta);
+    VectorXd delta_from_d = exp_Imu(d_from_delta);
     ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
 }
 
-TEST(IMU_tools, plus)
+TEST(Imu_tools, plus)
 {
     VectorXd d1(10), d2(10), d3(10);
     VectorXd err(9);
@@ -162,7 +162,7 @@ TEST(IMU_tools, plus)
     ASSERT_MATRIX_APPROX(diff(d3, d2), VectorXd::Zero(9), 1e-10);
 }
 
-TEST(IMU_tools, diff)
+TEST(Imu_tools, diff)
 {
     VectorXd d1(10), d2(10);
     Vector4d qv = (Vector4d() << 3, 4, 5, 6).finished().normalized();
@@ -184,7 +184,7 @@ TEST(IMU_tools, diff)
 
 }
 
-TEST(IMU_tools, compose_jacobians)
+TEST(Imu_tools, compose_jacobians)
 {
     VectorXd d1(10), d2(10), d3(10), d1_pert(10), d2_pert(10), d3_pert(10); // deltas
     VectorXd t1(9), t2(9); // tangents
@@ -224,7 +224,7 @@ TEST(IMU_tools, compose_jacobians)
     ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
 }
 
-TEST(IMU_tools, diff_jacobians)
+TEST(Imu_tools, diff_jacobians)
 {
     VectorXd d1(10), d2(10), d3(9), d1_pert(10), d2_pert(10), d3_pert(9); // deltas
     VectorXd t1(9), t2(9); // tangents
@@ -263,7 +263,7 @@ TEST(IMU_tools, diff_jacobians)
     ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
 }
 
-TEST(IMU_tools, body2delta_jacobians)
+TEST(Imu_tools, body2delta_jacobians)
 {
     VectorXd delta(10), delta_pert(10); // deltas
     VectorXd body(6), pert(6);
@@ -393,7 +393,7 @@ TEST(motion2data, AllRandom)
  *   N: number of steps
  *   q0: initial orientaiton
  *   motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame
- *   bias_real: the real bias of the IMU
+ *   bias_real: the real bias of the Imu
  *   bias_preint: the bias used for Delta pre-integration
  * Output:
  *   return: the preintegrated delta
@@ -423,7 +423,7 @@ VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, co
  *   N: number of steps
  *   q0: initial orientaiton
  *   motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame
- *   bias_real: the real bias of the IMU
+ *   bias_real: the real bias of the Imu
  *   bias_preint: the bias used for Delta pre-integration
  * Output:
  *   J_D_b: the Jacobian of the preintegrated delta wrt the bias
@@ -448,12 +448,12 @@ VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, co
         data = motion2data(motion, q, bias_real);
         q    = q*exp_q(motion.tail(3)*dt);
         // Motion::integrateOneStep()
-        {   // IMU::computeCurrentDelta
+        {   // Imu::computeCurrentDelta
             body  = data - bias_preint;
             body2delta(body, dt, delta, J_d_d);
             J_d_b = - J_d_d;
         }
-        {   // IMU::deltaPlusDelta
+        {   // Imu::deltaPlusDelta
             compose(Delta, delta, dt, Delta_plus, J_D_D, J_D_d);
         }
         // Motion:: jac calib
@@ -464,7 +464,7 @@ VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, co
     return Delta;
 }
 
-TEST(IMU_tools, integral_jacobian_bias)
+TEST(Imu_tools, integral_jacobian_bias)
 {
     VectorXd Delta(10), Delta_pert(10); // deltas
     VectorXd bias_real(6), pert(6), bias_pert(6), bias_preint(6);
@@ -499,7 +499,7 @@ TEST(IMU_tools, integral_jacobian_bias)
     ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4);
 }
 
-TEST(IMU_tools, delta_correction)
+TEST(Imu_tools, delta_correction)
 {
     VectorXd Delta(10), Delta_preint(10), Delta_corr; // deltas
     VectorXd bias(6), pert(6), bias_preint(6);
@@ -536,7 +536,7 @@ TEST(IMU_tools, delta_correction)
     ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-5);
 }
 
-TEST(IMU_tools, full_delta_residual)
+TEST(Imu_tools, full_delta_residual)
 {
     VectorXd x1(10), x2(10);
     VectorXd Delta(10), Delta_preint(10), Delta_corr(10);
@@ -623,7 +623,7 @@ TEST(IMU_tools, full_delta_residual)
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
-//  ::testing::GTEST_FLAG(filter) = "IMU_tools.delta_correction";
+//  ::testing::GTEST_FLAG(filter) = "Imu_tools.delta_correction";
   return RUN_ALL_TESTS();
 }
 
diff --git a/test/gtest_processor_IMU.cpp b/test/gtest_processor_imu.cpp
similarity index 92%
rename from test/gtest_processor_IMU.cpp
rename to test/gtest_processor_imu.cpp
index 1f059b193f8e5fbf5f96f5024543223ffa0840c2..90786bb6472b4ea5e5f085e05413cd3657dc1c72 100644
--- a/test/gtest_processor_IMU.cpp
+++ b/test/gtest_processor_imu.cpp
@@ -5,10 +5,10 @@
  *      \author: jsola
  */
 
-#include "IMU/capture/capture_IMU.h"
-#include "IMU/processor/processor_IMU.h"
-#include "IMU/sensor/sensor_IMU.h"
-#include "IMU/internal/config.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/processor/processor_imu.h"
+#include "imu/sensor/sensor_imu.h"
+#include "imu/internal/config.h"
 
 #include "core/common/wolf.h"
 
@@ -24,7 +24,7 @@
 
 using namespace Eigen;
 
-class ProcessorIMUt : public testing::Test
+class ProcessorImut : public testing::Test
 {
 
     public: //These can be accessed in fixtures
@@ -37,7 +37,7 @@ class ProcessorIMUt : public testing::Test
         Vector6d data;
         Matrix6d data_cov;
         VectorXd x0;
-        std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr;
+        std::shared_ptr<wolf::CaptureImu> cap_imu_ptr;
 
     //a new of this will be created for each new test
     virtual void SetUp()
@@ -54,8 +54,8 @@ class ProcessorIMUt : public testing::Test
         // Wolf problem
         problem = Problem::create("POV", 3);
         Vector7d extrinsics = (Vector7d() << 0,0,0, 0,0,0,1).finished();
-        sensor_ptr = problem->installSensor("SensorIMU", "Main IMU", extrinsics,  wolf_root + "/demos/sensor_imu.yaml");
-        ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorIMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
+        sensor_ptr = problem->installSensor("SensorImu", "Main Imu", extrinsics,  wolf_root + "/demos/sensor_imu.yaml");
+        ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu.yaml");
         processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
 
         // Time and data variables
@@ -65,8 +65,8 @@ class ProcessorIMUt : public testing::Test
         // Set the origin
         x0.resize(10);
 
-        // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
-        cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6d::Zero());
+        // Create one capture to store the Imu data arriving from (sensor / callback / file / etc.)
+        cap_imu_ptr = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector6d::Zero());
     }
 
     virtual void TearDown()
@@ -83,18 +83,18 @@ class ProcessorIMUt : public testing::Test
     }
 };
 
-TEST(ProcessorIMU_constructors, ALL)
+TEST(ProcessorImu_constructors, ALL)
 {
     using namespace wolf;
 
-    //constructor with ProcessorIMUParamsPtr argument only
-    ProcessorParamsIMUPtr param_ptr = std::make_shared<ProcessorParamsIMU>();
+    //constructor with ProcessorImuParamsPtr argument only
+    ProcessorParamsImuPtr param_ptr = std::make_shared<ProcessorParamsImu>();
     param_ptr->max_time_span =   2.0;
     param_ptr->max_buff_length = 20000;
     param_ptr->dist_traveled =   2.0;
     param_ptr->angle_turned =    2.0;
 
-    ProcessorIMUPtr prc1 = std::make_shared<ProcessorIMU>(param_ptr);
+    ProcessorImuPtr prc1 = std::make_shared<ProcessorImu>(param_ptr);
     ASSERT_EQ(prc1->getMaxTimeSpan(), param_ptr->max_time_span);
     ASSERT_EQ(prc1->getMaxBuffLength(), param_ptr->max_buff_length);
     ASSERT_EQ(prc1->getDistTraveled(), param_ptr->dist_traveled);
@@ -104,23 +104,23 @@ TEST(ProcessorIMU_constructors, ALL)
     std::string wolf_root = _WOLF_IMU_ROOT_DIR;
     ProblemPtr problem = Problem::create("POV", 3);
     Vector7d extrinsics = (Vector7d()<<1,0,0, 0,0,0,1).finished();
-    SensorBasePtr sensor_ptr = problem->installSensor("SensorIMU", "Main IMU", extrinsics, wolf_root + "/demos/sensor_imu.yaml");
-    ProcessorParamsIMUPtr params_default = std::make_shared<ProcessorParamsIMU>();
-    ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorIMU", "IMU pre-integrator", sensor_ptr, params_default);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(),   params_default->max_time_span);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), params_default->max_buff_length);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(),  params_default->dist_traveled);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(),   params_default->angle_turned);
+    SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", "Main Imu", extrinsics, wolf_root + "/demos/sensor_imu.yaml");
+    ProcessorParamsImuPtr params_default = std::make_shared<ProcessorParamsImu>();
+    ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_ptr, params_default);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxTimeSpan(),   params_default->max_time_span);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxBuffLength(), params_default->max_buff_length);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getDistTraveled(),  params_default->dist_traveled);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getAngleTurned(),   params_default->angle_turned);
 
     //Factory constructor with yaml
-    processor_ptr = problem->installProcessor("ProcessorIMU", "Sec IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
-    ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(),   2.0);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), 20000);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(),  2.0);
-    ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(),   0.2);
+    processor_ptr = problem->installProcessor("ProcessorImu", "Sec Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu.yaml");
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxTimeSpan(),   2.0);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxBuffLength(), 20000);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getDistTraveled(),  2.0);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getAngleTurned(),   0.2);
 }
 
-TEST(ProcessorIMU, voteForKeyFrame)
+TEST(ProcessorImu, voteForKeyFrame)
 {
     using namespace wolf;
     using namespace Eigen;
@@ -134,14 +134,14 @@ TEST(ProcessorIMU, voteForKeyFrame)
     // Wolf problem
     ProblemPtr problem = Problem::create("POV", 3);
     Vector7d extrinsics = (Vector7d()<<1,0,0, 0,0,0,1).finished();
-    SensorBasePtr sensor_ptr = problem->installSensor("SensorIMU", "Main IMU", extrinsics,  wolf_root + "/demos/sensor_imu.yaml");
-    ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
+    SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", "Main Imu", extrinsics,  wolf_root + "/demos/sensor_imu.yaml");
+    ProcessorParamsImuPtr prc_imu_params = std::make_shared<ProcessorParamsImu>();
     prc_imu_params->max_time_span = 1;
     prc_imu_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
     prc_imu_params->dist_traveled = 1000000000;
     prc_imu_params->angle_turned = 1000000000;
     prc_imu_params->voting_active = true;
-    ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorIMU", "IMU pre-integrator", sensor_ptr, prc_imu_params);
+    ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_ptr, prc_imu_params);
     
     //setting origin
     VectorXd x0(10);
@@ -156,17 +156,17 @@ TEST(ProcessorIMU, voteForKeyFrame)
     Matrix6d data_cov(Matrix6d::Identity());
     data_cov(0,0) = 0.5;
 
-    // Create the captureIMU to store the IMU data arriving from (sensor / callback / file / etc.)
-    std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6d::Zero());
+    // Create the captureImu to store the Imu data arriving from (sensor / callback / file / etc.)
+    std::shared_ptr<wolf::CaptureImu> cap_imu_ptr = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector6d::Zero());
 
     //  Time  
     // we want more than one data to integrate otherwise covariance will be 0
-    double dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() - 0.1;
+    double dt = std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxTimeSpan() - 0.1;
     t.set(dt);
     cap_imu_ptr->setTimeStamp(t);
     sensor_ptr->process(cap_imu_ptr);
 
-    dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() + 0.1;
+    dt = std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxTimeSpan() + 0.1;
     t.set(dt);
     cap_imu_ptr->setTimeStamp(t);
     sensor_ptr->process(cap_imu_ptr);
@@ -204,7 +204,7 @@ TEST(ProcessorIMU, voteForKeyFrame)
 }
 
 
-TEST_F(ProcessorIMUt, acc_x)
+TEST_F(ProcessorImut, acc_x)
 {
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  0,0,0;
@@ -227,7 +227,7 @@ TEST_F(ProcessorIMUt, acc_x)
     ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
 }
 
-TEST_F(ProcessorIMUt, acc_y)
+TEST_F(ProcessorImut, acc_y)
 {
     // last part of this test fails with precision wolf::Constants::EPS_SMALL beacause error is in 1e-12
     // difference hier is that we integrate over 1ms periods
@@ -267,7 +267,7 @@ TEST_F(ProcessorIMUt, acc_y)
     ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
 }
 
-TEST_F(ProcessorIMUt, acc_z)
+TEST_F(ProcessorImut, acc_z)
 {
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  0,0,0;
@@ -304,7 +304,7 @@ TEST_F(ProcessorIMUt, acc_z)
     ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
 }
 
-TEST_F(ProcessorIMUt, check_Covariance)
+TEST_F(ProcessorImut, check_Covariance)
 {
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  0,0,0;
@@ -324,7 +324,7 @@ TEST_F(ProcessorIMUt, check_Covariance)
 //    ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXd::Zero(9,9), wolf::Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
 }
 
-TEST_F(ProcessorIMUt, gyro_x)
+TEST_F(ProcessorImut, gyro_x)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -367,7 +367,7 @@ TEST_F(ProcessorIMUt, gyro_x)
     ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
 }
 
-TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
+TEST_F(ProcessorImut, gyro_x_biasedAbx)
 {
     // time
     double dt(0.001);
@@ -428,7 +428,7 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
 
 }
 
-TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
+TEST_F(ProcessorImut, gyro_xy_biasedAbxy)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -481,7 +481,7 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
 //    "\n expected state is : \n" << x.transpose() << std::endl;
 }
 
-TEST_F(ProcessorIMUt, gyro_z)
+TEST_F(ProcessorImut, gyro_z)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -520,7 +520,7 @@ TEST_F(ProcessorIMUt, gyro_z)
     ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
 }
 
-TEST_F(ProcessorIMUt, gyro_xyz)
+TEST_F(ProcessorImut, gyro_xyz)
 {
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  0,0,0;
@@ -565,7 +565,7 @@ TEST_F(ProcessorIMUt, gyro_xyz)
         // quaternion composition
         quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
         R0 = R0 * wolf::v2R(tmp_vec*dt);
-        // use processorIMU
+        // use processorImu
         Quaterniond rot(problem->getCurrentState().data()+3);
         data.head(3) =  rot.conjugate() * (- wolf::gravity()); //gravity measured
         data.tail(3) = tmp_vec;
@@ -578,7 +578,7 @@ TEST_F(ProcessorIMUt, gyro_xyz)
     /* We focus on orientation here. position is supposed not to have moved
      * we integrated using 2 ways : 
         - a direct quaternion composition q = q (x) q(w*dt)
-        - using methods implemented in processorIMU
+        - using methods implemented in processorImu
 
         We check one against the other
      */
@@ -606,7 +606,7 @@ TEST_F(ProcessorIMUt, gyro_xyz)
     ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), wolf::Constants::EPS);
 }
 
-TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
+TEST_F(ProcessorImut, gyro_z_ConstVelocity)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -645,7 +645,7 @@ TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
     ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
 }
 
-TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
+TEST_F(ProcessorImut, gyro_x_ConstVelocity)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -689,7 +689,7 @@ TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
     ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
 }
 
-TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
+TEST_F(ProcessorImut, gyro_xy_ConstVelocity)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -733,7 +733,7 @@ TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
     ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
 }
 
-TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
+TEST_F(ProcessorImut, gyro_y_ConstVelocity)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -777,7 +777,7 @@ TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
     ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
 }
 
-TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
+TEST_F(ProcessorImut, gyro_xyz_ConstVelocity)
 {
     t.set(0); // clock in 0,1 ms ticks
     x0 << 0,0,0,  0,0,0,1,  2,0,0;
@@ -822,7 +822,7 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
         // quaternion composition
         quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
         R0 = R0 * wolf::v2R(tmp_vec*dt);
-        // use processorIMU
+        // use processorImu
         Quaterniond rot(problem->getCurrentState().data()+3);
         data.head(3) =  rot.conjugate() * (- wolf::gravity()); //gravity measured
         data.tail(3) = tmp_vec;
@@ -835,7 +835,7 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
     /* We focus on orientation here. position is supposed not to have moved
      * we integrated using 2 ways : 
         - a direct quaternion composition q = q (x) q(w*dt)
-        - using methods implemented in processorIMU
+        - using methods implemented in processorImu
 
         We check one against the other
      */
@@ -865,7 +865,7 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
 
 }
 
-TEST_F(ProcessorIMUt, gyro_x_acc_x)
+TEST_F(ProcessorImut, gyro_x_acc_x)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -914,7 +914,7 @@ TEST_F(ProcessorIMUt, gyro_x_acc_x)
     ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
 }
 
-TEST_F(ProcessorIMUt, gyro_y_acc_y)
+TEST_F(ProcessorImut, gyro_y_acc_y)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -963,7 +963,7 @@ TEST_F(ProcessorIMUt, gyro_y_acc_y)
     ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
 }
 
-TEST_F(ProcessorIMUt, gyro_z_acc_z)
+TEST_F(ProcessorImut, gyro_z_acc_z)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -1014,7 +1014,7 @@ TEST_F(ProcessorIMUt, gyro_z_acc_z)
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
-  //::testing::GTEST_FLAG(filter) = "ProcessorIMUt.check_Covariance";
+  //::testing::GTEST_FLAG(filter) = "ProcessorImut.check_Covariance";
   return RUN_ALL_TESTS();
 }
 
diff --git a/test/gtest_processor_IMU_jacobians.cpp b/test/gtest_processor_imu_jacobians.cpp
similarity index 92%
rename from test/gtest_processor_IMU_jacobians.cpp
rename to test/gtest_processor_imu_jacobians.cpp
index 19a7b783fbb6bc99ca5710ccb52253a02185d335..e9fd58e521a716cd783a93f0994023068551f201 100644
--- a/test/gtest_processor_IMU_jacobians.cpp
+++ b/test/gtest_processor_imu_jacobians.cpp
@@ -6,9 +6,9 @@
  */
 
  //Wolf
-#include "IMU/capture/capture_IMU.h"
-#include "IMU/sensor/sensor_IMU.h"
-#include "processor_IMU_UnitTester.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/sensor/sensor_imu.h"
+#include "processor_imu_UnitTester.h"
 #include "core/common/wolf.h"
 #include "core/problem/problem.h"
 #include "core/state_block/state_block.h"
@@ -28,7 +28,7 @@
 using namespace wolf;
 
 // A new one of these is created for each test
-class ProcessorIMU_jacobians : public testing::Test
+class ProcessorImu_jacobians : public testing::Test
 {
     public:
         TimeStamp t;
@@ -38,16 +38,16 @@ class ProcessorIMU_jacobians : public testing::Test
         double dt;
         Eigen::Matrix<double,9,1> Delta_noise;
         Eigen::Matrix<double,9,1> delta_noise;
-        struct IMU_jac_bias bias_jac;
-        struct IMU_jac_deltas deltas_jac;
+        struct Imu_jac_bias bias_jac;
+        struct Imu_jac_deltas deltas_jac;
 
-        void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq0, Eigen::Map<Eigen::Quaterniond>& _dq0){
+        void remapJacDeltas_quat0(Imu_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq0, Eigen::Map<Eigen::Quaterniond>& _dq0){
 
             new (&_Dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta0_.data() + 3);
             new (&_dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.delta0_.data() + 3);
         }
 
-        void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq, Eigen::Map<Eigen::Quaterniond>& _dq, const int& place ){
+        void remapJacDeltas_quat(Imu_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq, Eigen::Map<Eigen::Quaterniond>& _dq, const int& place ){
     
             assert(place < _jac_delta.Delta_noisy_vect_.size());
             new (&_Dq) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
@@ -62,10 +62,10 @@ class ProcessorIMU_jacobians : public testing::Test
 
         // Wolf problem
         ProblemPtr wolf_problem_ptr_ = Problem::create("POV", 3);
-        Eigen::VectorXd IMU_extrinsics(7);
-        IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
+        Eigen::VectorXd Imu_extrinsics(7);
+        Imu_extrinsics << 0,0,0, 0,0,0,1; // Imu pose in the robot
 
-        ProcessorIMU_UnitTester processor_imu;
+        ProcessorImu_UnitTester processor_imu;
         ddelta_bias = 0.00000001;
         dt = 0.001;
 
@@ -86,13 +86,13 @@ class ProcessorIMU_jacobians : public testing::Test
         //std::cout << "\n rotation vector we start with :\n" << ang << std::endl;
 
         //get data to compute jacobians
-        struct IMU_jac_bias bias_jac_c = processor_imu.finite_diff_ab(dt, data_, ddelta_bias, Delta0);
+        struct Imu_jac_bias bias_jac_c = processor_imu.finite_diff_ab(dt, data_, ddelta_bias, Delta0);
         bias_jac.copyfrom(bias_jac_c);
 
         Delta_noise << 0.00000001, 0.00000001, 0.00000001,  0.0001, 0.0001, 0.0001,  0.00000001, 0.00000001, 0.00000001;
         delta_noise << 0.00000001, 0.00000001, 0.00000001,  0.0001, 0.0001, 0.0001,  0.00000001, 0.00000001, 0.00000001;
 
-        struct IMU_jac_deltas deltas_jac_c = processor_imu.finite_diff_noise(dt, data_, Delta_noise, delta_noise, Delta0);
+        struct Imu_jac_deltas deltas_jac_c = processor_imu.finite_diff_noise(dt, data_, Delta_noise, delta_noise, Delta0);
         deltas_jac = deltas_jac_c;
     }
 
@@ -110,7 +110,7 @@ class ProcessorIMU_jacobians : public testing::Test
     }
 };
 
-class ProcessorIMU_jacobians_Dq : public testing::Test
+class ProcessorImu_jacobians_Dq : public testing::Test
 {
     public:
         TimeStamp t;
@@ -118,15 +118,15 @@ class ProcessorIMU_jacobians_Dq : public testing::Test
         Eigen::Matrix<double,10,1> Delta0;
         double ddelta_bias2;
         double dt;
-        struct IMU_jac_bias bias_jac2;
+        struct Imu_jac_bias bias_jac2;
 
-        void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq0, Eigen::Map<Eigen::Quaterniond>& _dq0){
+        void remapJacDeltas_quat0(Imu_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq0, Eigen::Map<Eigen::Quaterniond>& _dq0){
 
             new (&_Dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta0_.data() + 3);
             new (&_dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.delta0_.data() + 3);
         }
 
-        void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq, Eigen::Map<Eigen::Quaterniond>& _dq, const int& place ){
+        void remapJacDeltas_quat(Imu_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq, Eigen::Map<Eigen::Quaterniond>& _dq, const int& place ){
     
             assert(place < _jac_delta.Delta_noisy_vect_.size());
             new (&_Dq) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
@@ -141,10 +141,10 @@ class ProcessorIMU_jacobians_Dq : public testing::Test
 
         // Wolf problem
         ProblemPtr wolf_problem_ptr_ = Problem::create("POV", 3);
-        Eigen::VectorXd IMU_extrinsics(7);
-        IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
+        Eigen::VectorXd Imu_extrinsics(7);
+        Imu_extrinsics << 0,0,0, 0,0,0,1; // Imu pose in the robot
 
-        ProcessorIMU_UnitTester processor_imu;
+        ProcessorImu_UnitTester processor_imu;
         ddelta_bias2 = 0.0001;
         dt = 0.001;
 
@@ -165,7 +165,7 @@ class ProcessorIMU_jacobians_Dq : public testing::Test
         //std::cout << "\n rotation vector we start with :\n" << ang << std::endl;
 
         //get data to compute jacobians
-        struct IMU_jac_bias bias_jac_c = processor_imu.finite_diff_ab(dt, data_, ddelta_bias2, Delta0);
+        struct Imu_jac_bias bias_jac_c = processor_imu.finite_diff_ab(dt, data_, ddelta_bias2, Delta0);
         bias_jac2.copyfrom(bias_jac_c);
     }
 
@@ -185,7 +185,7 @@ class ProcessorIMU_jacobians_Dq : public testing::Test
 
                             ///BIAS TESTS
 
-/*                              IMU_jac_deltas struct form :
+/*                              Imu_jac_deltas struct form :
     contains vectors of size 7 :
     Elements at place 0 are those not affected by the bias noise that we add (da_bx,..., dw_bx,... ).
                 place 1 : added da_bx in data         place 2 : added da_by in data       place 3 : added da_bz in data
@@ -215,7 +215,7 @@ class ProcessorIMU_jacobians_Dq : public testing::Test
         Then at first step, dR.tr() = Id, dDq_dwb = 0_{3x3}, which boils down to dDq_dwb_ = Jr(wdt)*dt 
 */
 
-TEST_F(ProcessorIMU_jacobians, dDp_dab)
+TEST_F(ProcessorImu_jacobians, dDp_dab)
 {
     using namespace wolf;
     Eigen::Matrix3d dDp_dab;
@@ -227,7 +227,7 @@ TEST_F(ProcessorIMU_jacobians, dDp_dab)
      "\ndDp_dab_a - dDp_dab_ : \n" << bias_jac.dDp_dab_ - dDp_dab << std::endl;
 }
 
-TEST_F(ProcessorIMU_jacobians, dDv_dab)
+TEST_F(ProcessorImu_jacobians, dDv_dab)
 {
     using namespace wolf;
     Eigen::Matrix3d dDv_dab;
@@ -239,7 +239,7 @@ TEST_F(ProcessorIMU_jacobians, dDv_dab)
      "\ndDv_dab_a - dDv_dab_ : \n" << bias_jac.dDv_dab_ - dDv_dab << std::endl;
 }
 
-TEST_F(ProcessorIMU_jacobians, dDp_dwb)
+TEST_F(ProcessorImu_jacobians, dDp_dwb)
 {
     using namespace wolf;
     Eigen::Matrix3d dDp_dwb;
@@ -251,7 +251,7 @@ TEST_F(ProcessorIMU_jacobians, dDp_dwb)
      "\ndDp_dwb_a - dDv_dab_ : \n" << bias_jac.dDp_dwb_ - dDp_dwb << std::endl;
 }
 
-TEST_F(ProcessorIMU_jacobians, dDv_dwb_)
+TEST_F(ProcessorImu_jacobians, dDv_dwb_)
 {
     using namespace wolf;
     Eigen::Matrix3d dDv_dwb;
@@ -263,7 +263,7 @@ TEST_F(ProcessorIMU_jacobians, dDv_dwb_)
      "\ndDv_dwb_a - dDv_dwb_ : \n" << bias_jac.dDv_dwb_ - dDv_dwb << std::endl;
 }
 
-TEST_F(ProcessorIMU_jacobians, dDq_dab)
+TEST_F(ProcessorImu_jacobians, dDq_dab)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> q_in_1(NULL), q_in_2(NULL);
@@ -278,7 +278,7 @@ TEST_F(ProcessorIMU_jacobians, dDq_dab)
     ASSERT_TRUE(dDq_dab.isZero(wolf::Constants::EPS)) << "\t\tdDq_dab_ jacobian is not Zero :" << dDq_dab << std::endl;
 }
 
-TEST_F(ProcessorIMU_jacobians, dDq_dwb_noise_1Em8_)
+TEST_F(ProcessorImu_jacobians, dDq_dwb_noise_1Em8_)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> q_in_1(NULL), q_in_2(NULL);
@@ -300,7 +300,7 @@ TEST_F(ProcessorIMU_jacobians, dDq_dwb_noise_1Em8_)
         << std::endl;
 }
 
-TEST_F(ProcessorIMU_jacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
+TEST_F(ProcessorImu_jacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> q_in_1(NULL), q_in_2(NULL);
@@ -394,7 +394,7 @@ TEST_F(ProcessorIMU_jacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
     dDo_doz = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(0,0,dthetaz)) )/dthetaz
      */
      
-TEST_F(ProcessorIMU_jacobians, dDp_dP)
+TEST_F(ProcessorImu_jacobians, dDp_dP)
 {
     using namespace wolf;
     Eigen::Matrix3d dDp_dP;
@@ -407,7 +407,7 @@ TEST_F(ProcessorIMU_jacobians, dDp_dP)
      "\ndDp_dP_a - dDp_dP_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,0,3,3) - dDp_dP << std::endl;
 }
 
-TEST_F(ProcessorIMU_jacobians, dDp_dV)
+TEST_F(ProcessorImu_jacobians, dDp_dV)
 {
     using namespace wolf;
     Eigen::Matrix3d dDp_dV;
@@ -420,7 +420,7 @@ TEST_F(ProcessorIMU_jacobians, dDp_dV)
      "\ndDp_dV_a - dDp_dV_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,6,3,3) - dDp_dV << std::endl;
 }
 
-TEST_F(ProcessorIMU_jacobians, dDp_dO)
+TEST_F(ProcessorImu_jacobians, dDp_dO)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
@@ -437,7 +437,7 @@ TEST_F(ProcessorIMU_jacobians, dDp_dO)
      "\ndDp_dO_a - dDp_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,3,3,3) - dDp_dO << std::endl;
 }
 
-TEST_F(ProcessorIMU_jacobians, dDv_dV)
+TEST_F(ProcessorImu_jacobians, dDv_dV)
 {
     using namespace wolf;
     Eigen::Matrix3d dDv_dV;
@@ -450,7 +450,7 @@ TEST_F(ProcessorIMU_jacobians, dDv_dV)
      "\ndDv_dV_a - dDv_dV_ : \n" << deltas_jac.jacobian_delta_preint_.block(6,6,3,3) - dDv_dV << std::endl;
 }
 
-TEST_F(ProcessorIMU_jacobians, dDv_dO)
+TEST_F(ProcessorImu_jacobians, dDv_dO)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
@@ -469,7 +469,7 @@ TEST_F(ProcessorIMU_jacobians, dDv_dO)
 
 //dDo_dP = dDo_dV = [0, 0, 0]
 
-TEST_F(ProcessorIMU_jacobians, dDo_dO)
+TEST_F(ProcessorImu_jacobians, dDo_dO)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
@@ -486,7 +486,7 @@ TEST_F(ProcessorIMU_jacobians, dDo_dO)
      "\ndDo_dO_a - dDo_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(3,3,3,3) - dDo_dO << std::endl;
 }
 
-TEST_F(ProcessorIMU_jacobians, dDp_dp)
+TEST_F(ProcessorImu_jacobians, dDp_dp)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL);
@@ -504,7 +504,7 @@ TEST_F(ProcessorIMU_jacobians, dDp_dp)
 
 //dDv_dp = [0, 0, 0]
 
-TEST_F(ProcessorIMU_jacobians, dDv_dv)
+TEST_F(ProcessorImu_jacobians, dDv_dv)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL);
@@ -522,7 +522,7 @@ TEST_F(ProcessorIMU_jacobians, dDv_dv)
 
 //dDo_dp = dDo_dv = [0, 0, 0]
 
-TEST_F(ProcessorIMU_jacobians, dDo_do)
+TEST_F(ProcessorImu_jacobians, dDo_do)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
diff --git a/test/processor_IMU_UnitTester.cpp b/test/processor_IMU_UnitTester.cpp
deleted file mode 100644
index aa4f46db66546395e9b4d07920407e2c0b52a597..0000000000000000000000000000000000000000
--- a/test/processor_IMU_UnitTester.cpp
+++ /dev/null
@@ -1,13 +0,0 @@
-#include "processor_IMU_UnitTester.h"
-
-namespace wolf {
-
-ProcessorIMU_UnitTester::ProcessorIMU_UnitTester() :
-        ProcessorIMU(std::make_shared<ProcessorParamsIMU>()),
-        Dq_out_(nullptr)
-{}
-
-ProcessorIMU_UnitTester::~ProcessorIMU_UnitTester(){}
-
-} // namespace wolf
-
diff --git a/test/processor_imu_UnitTester.cpp b/test/processor_imu_UnitTester.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7da3afb9f5c98329ad469d564691adad9ab40dba
--- /dev/null
+++ b/test/processor_imu_UnitTester.cpp
@@ -0,0 +1,13 @@
+#include "processor_imu_UnitTester.h"
+
+namespace wolf {
+
+ProcessorImu_UnitTester::ProcessorImu_UnitTester() :
+        ProcessorImu(std::make_shared<ProcessorParamsImu>()),
+        Dq_out_(nullptr)
+{}
+
+ProcessorImu_UnitTester::~ProcessorImu_UnitTester(){}
+
+} // namespace wolf
+
diff --git a/test/processor_IMU_UnitTester.h b/test/processor_imu_UnitTester.h
similarity index 93%
rename from test/processor_IMU_UnitTester.h
rename to test/processor_imu_UnitTester.h
index ccfc60fd4579b33786990d1ca2c888920c12a15a..2e9bbdd3a61a6619f7f84b47e202a4da848ff459 100644
--- a/test/processor_IMU_UnitTester.h
+++ b/test/processor_imu_UnitTester.h
@@ -3,13 +3,13 @@
 #define PROCESSOR_IMU_UNITTESTER_H
 
 // Wolf
-#include "IMU/processor/processor_IMU.h"
+#include "imu/processor/processor_imu.h"
 #include "core/processor/processor_motion.h"
 
 namespace wolf {
-    struct IMU_jac_bias{ //struct used for checking jacobians by finite difference
+    struct Imu_jac_bias{ //struct used for checking jacobians by finite difference
 
-        IMU_jac_bias(Eigen::Matrix<Eigen::VectorXd,6,1> _Deltas_noisy_vect,
+        Imu_jac_bias(Eigen::Matrix<Eigen::VectorXd,6,1> _Deltas_noisy_vect,
                      Eigen::VectorXd _Delta0 ,
                      Eigen::Matrix3d _dDp_dab,
                      Eigen::Matrix3d _dDv_dab,
@@ -27,7 +27,7 @@ namespace wolf {
             //
         }
                 
-        IMU_jac_bias(){
+        Imu_jac_bias(){
 
             for (int i=0; i<6; i++){
                 Deltas_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1);
@@ -41,7 +41,7 @@ namespace wolf {
             dDq_dwb_ = Eigen::Matrix3d::Zero();
         }
 
-        IMU_jac_bias(IMU_jac_bias const & toCopy){
+        Imu_jac_bias(Imu_jac_bias const & toCopy){
 
             Deltas_noisy_vect_ = toCopy.Deltas_noisy_vect_;
             Delta0_  = toCopy.Delta0_;
@@ -66,7 +66,7 @@ namespace wolf {
             Eigen::Matrix3d dDq_dwb_;
 
         public:
-            void copyfrom(IMU_jac_bias const& right){
+            void copyfrom(Imu_jac_bias const& right){
 
                 Deltas_noisy_vect_ = right.Deltas_noisy_vect_;
                 Delta0_  = right.Delta0_;
@@ -78,9 +78,9 @@ namespace wolf {
             }
     };
 
-    struct IMU_jac_deltas{
+    struct Imu_jac_deltas{
 
-        IMU_jac_deltas(Eigen::VectorXd _Delta0,
+        Imu_jac_deltas(Eigen::VectorXd _Delta0,
                        Eigen::VectorXd _delta0,
                        Eigen::Matrix<Eigen::VectorXd,9,1> _Delta_noisy_vect,
                        Eigen::Matrix<Eigen::VectorXd,9,1> _delta_noisy_vect,
@@ -96,7 +96,7 @@ namespace wolf {
             //
         }
 
-        IMU_jac_deltas(){
+        Imu_jac_deltas(){
             for (int i=0; i<9; i++){
                 Delta_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1);
                 delta_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1);
@@ -108,7 +108,7 @@ namespace wolf {
             jacobian_delta_ = Eigen::MatrixXd::Zero(9,9);
         }
 
-        IMU_jac_deltas(IMU_jac_deltas const & toCopy){
+        Imu_jac_deltas(Imu_jac_deltas const & toCopy){
 
             Delta_noisy_vect_ = toCopy.Delta_noisy_vect_;
             delta_noisy_vect_ = toCopy.delta_noisy_vect_;
@@ -136,7 +136,7 @@ namespace wolf {
             Eigen::MatrixXd jacobian_delta_;
 
         public:
-            void copyfrom(IMU_jac_deltas const& right){
+            void copyfrom(Imu_jac_deltas const& right){
 
                 Delta_noisy_vect_       = right.Delta_noisy_vect_;
                 delta_noisy_vect_       = right.delta_noisy_vect_;
@@ -147,32 +147,32 @@ namespace wolf {
             }
     };
 
-    class ProcessorIMU_UnitTester : public ProcessorIMU{
+    class ProcessorImu_UnitTester : public ProcessorImu{
 
         public:
 
-        ProcessorIMU_UnitTester();
-        virtual ~ProcessorIMU_UnitTester();
+        ProcessorImu_UnitTester();
+        virtual ~ProcessorImu_UnitTester();
 
         //Functions to test jacobians with finite difference method
 
         /* params :
             _data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
-            _dt : time interval between 2 IMU measurements
+            _dt : time interval between 2 Imu measurements
             da_b : bias noise to add - scalar because adding the same noise to each component of bias (abx, aby, abz, wbx, wby, wbz) one by one. 
          */
-        IMU_jac_bias finite_diff_ab(const double _dt,
+        Imu_jac_bias finite_diff_ab(const double _dt,
                                     Eigen::Vector6d& _data,
                                     const double& da_b,
                                     const Eigen::Matrix<double,10,1>& _delta_preint0);
 
         /* params :
             _data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
-            _dt : time interval between 2 IMU measurements
+            _dt : time interval between 2 Imu measurements
             _Delta_noise : noise to add to Delta_preint (D1 in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix()))
             _delta_noise : noise to add to instantaneous delta (d in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix()))
          */
-        IMU_jac_deltas finite_diff_noise(const double& _dt,
+        Imu_jac_deltas finite_diff_noise(const double& _dt,
                                          Eigen::Vector6d& _data,
                                          const Eigen::Matrix<double,9,1>& _Delta_noise,
                                          const Eigen::Matrix<double,9,1>& _delta_noise,
@@ -202,7 +202,7 @@ namespace wolf {
 namespace wolf{
 
     //Functions to test jacobians with finite difference method
-inline IMU_jac_bias ProcessorIMU_UnitTester::finite_diff_ab(const double _dt,
+inline Imu_jac_bias ProcessorImu_UnitTester::finite_diff_ab(const double _dt,
                                                             Eigen::Vector6d& _data,
                                                             const double& da_b,
                                                             const Eigen::Matrix<double,10,1>& _delta_preint0)
@@ -265,11 +265,11 @@ inline IMU_jac_bias ProcessorIMU_UnitTester::finite_diff_ab(const double _dt,
         Deltas_noisy_vect(i) = delta_preint_plus_delta0; //preintegrated deltas affected by added bias noise
     }
 
-    IMU_jac_bias bias_jacobians(Deltas_noisy_vect, Delta0, dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dwb);
+    Imu_jac_bias bias_jacobians(Deltas_noisy_vect, Delta0, dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dwb);
     return bias_jacobians;
 }
 
-inline IMU_jac_deltas ProcessorIMU_UnitTester::finite_diff_noise(const double& _dt, Eigen::Vector6d& _data, const Eigen::Matrix<double,9,1>& _Delta_noise, const Eigen::Matrix<double,9,1>& _delta_noise, const Eigen::Matrix<double,10,1>& _Delta0)
+inline Imu_jac_deltas ProcessorImu_UnitTester::finite_diff_noise(const double& _dt, Eigen::Vector6d& _data, const Eigen::Matrix<double,9,1>& _Delta_noise, const Eigen::Matrix<double,9,1>& _delta_noise, const Eigen::Matrix<double,10,1>& _Delta0)
 {
     //we do not propagate any noise from data vector
     Eigen::VectorXd Delta_; //will contain the preintegrated Delta affected by added noise
@@ -369,11 +369,11 @@ inline IMU_jac_deltas ProcessorIMU_UnitTester::finite_diff_noise(const double& _
         Delta_noisy_vect(i+3) = Delta_;
     }
     
-    IMU_jac_deltas jac_deltas(_Delta0, delta0, Delta_noisy_vect, delta_noisy_vect, jacobian_delta_preint0, jacobian_delta0);
+    Imu_jac_deltas jac_deltas(_Delta0, delta0, Delta_noisy_vect, delta_noisy_vect, jacobian_delta_preint0, jacobian_delta0);
     return jac_deltas;
 
 }
 
 } // namespace wolf
 
-#endif // PROCESSOR_IMU_UNITTESTER_H
+#endif // PROCESSOR_Imu_UNITTESTER_H