diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 22d70f19a0deea4dc27652bf00b1428140cd2270..9adb719715a0188f8c55a50d5d4443b92f829645 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -231,8 +231,8 @@ SET(HDRS_BASE
 
 SET(HDRS
     capture_pose.h
-    capture_gps_fix.h
-    capture_imu.h
+    capture_GPS_fix.h
+    capture_IMU.h
     capture_odom_2D.h
     capture_odom_3D.h
     capture_void.h
@@ -241,11 +241,11 @@ SET(HDRS
     constraint_corner_2D.h
     constraint_AHP.h
     constraint_epipolar.h
-    constraint_imu.h
+    constraint_IMU.h
     constraint_fix_bias.h
-    constraint_gps_2D.h
-    constraint_gps_pseudorange_3D.h
-    constraint_gps_pseudorange_2D.h
+    constraint_GPS_2D.h
+    constraint_GPS_pseudorange_3D.h
+    constraint_GPS_pseudorange_2D.h
     constraint_odom_2D.h
     constraint_odom_2D_analytic.h
     constraint_odom_3D.h
@@ -256,9 +256,9 @@ SET(HDRS
     constraint_quaternion_absolute.h
     constraint_relative_2D_analytic.h
     feature_corner_2D.h
-    feature_gps_fix.h
-    feature_gps_pseudorange.h
-    feature_imu.h
+    feature_GPS_fix.h
+    feature_GPS_pseudorange.h
+    feature_IMU.h
     feature_odom_2D.h
     feature_polyline_2D.h
     feature_pose.h
@@ -269,20 +269,20 @@ SET(HDRS
     local_parametrization_polyline_extreme.h
     processor_frame_nearest_neighbor_filter.h
     processor_image_params.h
-    processor_imu.h
-    test/processor_imu_UnitTester.h
+    processor_IMU.h
+    test/processor_IMU_UnitTester.h
     processor_odom_2D.h
     processor_odom_3D.h
     processor_tracker_feature_dummy.h
     processor_tracker_landmark_dummy.h
     sensor_camera.h
-    sensor_gps.h
-    sensor_gps_fix.h
-    sensor_imu.h
+    sensor_GPS.h
+    sensor_GPS_fix.h
+    sensor_IMU.h
     sensor_odom_2D.h
     sensor_odom_3D.h
     )
-
+# [Add generic derived header before this line]
 
 SET(HDRS_DTASSC
     data_association/matrix.h
@@ -322,16 +322,16 @@ SET(SRCS_BASE
     )
 
 SET(SRCS
-    capture_gps_fix.cpp
-    capture_imu.cpp
+    capture_GPS_fix.cpp
+    capture_IMU.cpp
     capture_odom_2D.cpp
     capture_odom_3D.cpp
     capture_pose.cpp
     capture_void.cpp
     feature_corner_2D.cpp
-    feature_gps_fix.cpp
-    feature_gps_pseudorange.cpp
-    feature_imu.cpp
+    feature_GPS_fix.cpp
+    feature_GPS_pseudorange.cpp
+    feature_IMU.cpp
     feature_odom_2D.cpp
     feature_polyline_2D.cpp
     feature_pose.cpp
@@ -341,16 +341,16 @@ SET(SRCS
     landmark_polyline_2D.cpp
     local_parametrization_polyline_extreme.cpp
     processor_frame_nearest_neighbor_filter.cpp
-    processor_imu.cpp
-    test/processor_imu_UnitTester.cpp
+    processor_IMU.cpp
+    test/processor_IMU_UnitTester.cpp
     processor_odom_2D.cpp
     processor_odom_3D.cpp
     processor_tracker_feature_dummy.cpp
     processor_tracker_landmark_dummy.cpp
     sensor_camera.cpp
-    sensor_gps.cpp
-    sensor_gps_fix.cpp
-    sensor_imu.cpp
+    sensor_GPS.cpp
+    sensor_GPS_fix.cpp
+    sensor_IMU.cpp
     sensor_odom_2D.cpp
     sensor_odom_3D.cpp
     )
@@ -403,12 +403,12 @@ ENDIF(laser_scan_utils_FOUND)
 
 IF (raw_gps_utils_FOUND)
     SET(HDRS ${HDRS}
-        capture_gps.h
-        processor_gps.h
+        capture_GPS.h
+        processor_GPS.h
         )
     SET(SRCS ${SRCS}
-        capture_gps.cpp
-        processor_gps.cpp
+        capture_GPS.cpp
+        processor_GPS.cpp
         )
 ENDIF(raw_gps_utils_FOUND)
 
@@ -420,7 +420,7 @@ IF (OpenCV_FOUND)
         constraint_AHP.h
         feature_point_image.h
         landmark_AHP.h
-        landmark_point_3d.h
+        landmark_point_3D.h
         processor_image_feature.h
         processor_image_landmark.h
         )
@@ -429,7 +429,7 @@ IF (OpenCV_FOUND)
         capture_image.cpp
         feature_point_image.cpp
         landmark_AHP.cpp
-        landmark_point_3d.cpp
+        landmark_point_3D.cpp
         processor_image_feature.cpp
         processor_image_landmark.cpp
         )
@@ -471,10 +471,10 @@ IF(YAMLCPP_FOUND)
     # sources
     SET(SRCS ${SRCS}
         yaml/processor_odom_3D_yaml.cpp
-        yaml/processor_imu_yaml.cpp
+        yaml/processor_IMU_yaml.cpp
         yaml/sensor_camera_yaml.cpp
         yaml/sensor_odom_3D_yaml.cpp
-        yaml/sensor_imu_yaml.cpp
+        yaml/sensor_IMU_yaml.cpp
         )
     IF(laser_scan_utils_FOUND)
         SET(SRCS ${SRCS}
diff --git a/src/imu_tools.h b/src/IMU_tools.h
similarity index 100%
rename from src/imu_tools.h
rename to src/IMU_tools.h
diff --git a/src/capture_gps.cpp b/src/capture_GPS.cpp
similarity index 95%
rename from src/capture_gps.cpp
rename to src/capture_GPS.cpp
index d5a5dfbbccddb2af0eeba6d8b37078b592e9b1eb..fb9800932e7f033b46f5ccdbb33b8ae05f6ee2ac 100644
--- a/src/capture_gps.cpp
+++ b/src/capture_GPS.cpp
@@ -1,4 +1,4 @@
-#include "capture_gps.h"
+#include <capture_GPS.h>
 
 namespace wolf {
 
diff --git a/src/capture_gps.h b/src/capture_GPS.h
similarity index 100%
rename from src/capture_gps.h
rename to src/capture_GPS.h
diff --git a/src/capture_gps_fix.cpp b/src/capture_GPS_fix.cpp
similarity index 96%
rename from src/capture_gps_fix.cpp
rename to src/capture_GPS_fix.cpp
index 28009e521a7cb5b6cc0df92da1b1410a4c6f1c06..bf3fef5b993a22495857871c1bcfdafe4b05cc0f 100644
--- a/src/capture_gps_fix.cpp
+++ b/src/capture_GPS_fix.cpp
@@ -1,4 +1,4 @@
-#include "capture_gps_fix.h"
+#include <capture_GPS_fix.h>
 
 
 namespace wolf {
diff --git a/src/capture_gps_fix.h b/src/capture_GPS_fix.h
similarity index 96%
rename from src/capture_gps_fix.h
rename to src/capture_GPS_fix.h
index 3747eefafff56ddb43da60c430881e786ac16a5f..2ffbc08e53350dbbb408d0bc49bfea84fc1f6de8 100644
--- a/src/capture_gps_fix.h
+++ b/src/capture_GPS_fix.h
@@ -2,8 +2,8 @@
 #define CAPTURE_GPS_FIX_H_
 
 //Wolf includes
+#include <feature_GPS_fix.h>
 #include "capture_base.h"
-#include "feature_gps_fix.h"
 
 //std includes
 //
diff --git a/src/capture_imu.cpp b/src/capture_IMU.cpp
similarity index 95%
rename from src/capture_imu.cpp
rename to src/capture_IMU.cpp
index 1fd8d3ac93e4ba324f8e75a1300671c438b2d1cc..d497a0f77e1fbf33116f5df2a89603c4d857ca64 100644
--- a/src/capture_imu.cpp
+++ b/src/capture_IMU.cpp
@@ -1,5 +1,5 @@
-#include "sensor_imu.h"
-#include "capture_imu.h"
+#include <capture_IMU.h>
+#include <sensor_IMU.h>
 #include "state_quaternion.h"
 
 namespace wolf {
diff --git a/src/capture_imu.h b/src/capture_IMU.h
similarity index 97%
rename from src/capture_imu.h
rename to src/capture_IMU.h
index ff760eb9bd3ec0732267c65811c56116ccdeed42..e21b6bd5d7abd53f79dda618b15c2a8de6fe4cec 100644
--- a/src/capture_imu.h
+++ b/src/capture_IMU.h
@@ -2,8 +2,8 @@
 #define CAPTURE_IMU_H
 
 //Wolf includes
+#include <IMU_tools.h>
 #include "capture_motion.h"
-#include "imu_tools.h"
 
 namespace wolf {
     
diff --git a/src/capture_base.h b/src/capture_base.h
index 49bdc63f0252778ebf4d45cec542a547dcd2844e..02d4c5352d725597bf186962a1211969104df81f 100644
--- a/src/capture_base.h
+++ b/src/capture_base.h
@@ -42,8 +42,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
                     SensorBasePtr _sensor_ptr   = nullptr,
                     StateBlockPtr _p_ptr        = nullptr,
                     StateBlockPtr _o_ptr        = nullptr,
-                    StateBlockPtr _intr_ptr     = nullptr
-                    );
+                    StateBlockPtr _intr_ptr     = nullptr);
 
         virtual ~CaptureBase();
         void remove();
diff --git a/src/captures/CMakeLists.txt b/src/captures/CMakeLists.txt
index 8776bb7d109f7621a1b408a03836832993926dbc..c856a0c217f4f7c1bc631532498358be1a279736 100644
--- a/src/captures/CMakeLists.txt
+++ b/src/captures/CMakeLists.txt
@@ -3,9 +3,11 @@ INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
 # Forward var to parent scope
 
 SET(HDRS_CAPTURE ${HDRS_CAPTURE}
-                 ${CMAKE_CURRENT_SOURCE_DIR}/capture_velocity.h
-                 ${CMAKE_CURRENT_SOURCE_DIR}/capture_wheel_joint_position.h PARENT_SCOPE)
+  ${CMAKE_CURRENT_SOURCE_DIR}/capture_velocity.h
+  ${CMAKE_CURRENT_SOURCE_DIR}/capture_wheel_joint_position.h 
+  PARENT_SCOPE)
 
 SET(SRCS_CAPTURE ${SRCS_CAPTURE}
-                 ${CMAKE_CURRENT_SOURCE_DIR}/capture_velocity.cpp
-                 ${CMAKE_CURRENT_SOURCE_DIR}/capture_wheel_joint_position.cpp PARENT_SCOPE)
+  ${CMAKE_CURRENT_SOURCE_DIR}/capture_velocity.cpp
+  ${CMAKE_CURRENT_SOURCE_DIR}/capture_wheel_joint_position.cpp 
+  PARENT_SCOPE)
diff --git a/src/constraint_gps_2D.h b/src/constraint_GPS_2D.h
similarity index 100%
rename from src/constraint_gps_2D.h
rename to src/constraint_GPS_2D.h
diff --git a/src/constraint_gps_pseudorange_2D.h b/src/constraint_GPS_pseudorange_2D.h
similarity index 99%
rename from src/constraint_gps_pseudorange_2D.h
rename to src/constraint_GPS_pseudorange_2D.h
index d8fc2325623da21ceffb6c356d27f478aed37375..e0d8d2a9ae21eb868640350e1a5aaac564f51c44 100644
--- a/src/constraint_gps_pseudorange_2D.h
+++ b/src/constraint_GPS_pseudorange_2D.h
@@ -4,7 +4,7 @@
 #define LIGHT_SPEED_ 299792458
 
 //Wolf includes
-#include "sensor_gps.h"
+#include <sensor_GPS.h>
 #include "feature_gps_pseudorange.h"
 #include "constraint_autodiff.h"
 
diff --git a/src/constraint_gps_pseudorange_3D.h b/src/constraint_GPS_pseudorange_3D.h
similarity index 99%
rename from src/constraint_gps_pseudorange_3D.h
rename to src/constraint_GPS_pseudorange_3D.h
index 58f471aa33cc3c0ae21aa9564b6930d0c6d0da5e..97fb326c08ba76178248935fbd96164cd5266f12 100644
--- a/src/constraint_gps_pseudorange_3D.h
+++ b/src/constraint_GPS_pseudorange_3D.h
@@ -4,8 +4,8 @@
 #define LIGHT_SPEED 299792458
 
 //Wolf includes
-#include "sensor_gps.h"
-#include "feature_gps_pseudorange.h"
+#include <sensor_GPS.h>
+#include <feature_GPS_pseudorange.h>
 #include "constraint_autodiff.h"
 
 namespace wolf {
diff --git a/src/constraint_imu.h b/src/constraint_IMU.h
similarity index 99%
rename from src/constraint_imu.h
rename to src/constraint_IMU.h
index c61d7d720eab1cf27e44d9d0639931e6dc3ce52c..cd1843a7188e99b2bc0bcb8ef9991d96330ab5e4 100644
--- a/src/constraint_imu.h
+++ b/src/constraint_IMU.h
@@ -2,9 +2,9 @@
 #define CONSTRAINT_IMU_THETA_H_
 
 //Wolf includes
+#include <feature_IMU.h>
+#include <sensor_IMU.h>
 #include "constraint_autodiff.h"
-#include "feature_imu.h"
-#include "sensor_imu.h"
 #include "rotations.h"
 
 //Eigen include
diff --git a/src/constraint_autodiff.h b/src/constraint_autodiff.h
index 764dec532c8bca8e988b30203296488779a5457c..109f73623dd0b674afb8a8a57b80e98b5672631c 100644
--- a/src/constraint_autodiff.h
+++ b/src/constraint_autodiff.h
@@ -16,7 +16,7 @@ namespace wolf {
 
 //template class ConstraintAutodiff
 template <class CtrT, unsigned int RES, unsigned int B0, unsigned int B1 = 0, unsigned int B2 = 0, unsigned int B3 = 0, unsigned int B4 = 0, unsigned int B5 = 0, unsigned int B6 = 0, unsigned int B7 = 0, unsigned int B8 = 0, unsigned int B9 = 0>
-class ConstraintAutodiff: public ConstraintBase
+class ConstraintAutodiff : public ConstraintBase
 {
     public:
 
diff --git a/src/constraint_base.h b/src/constraint_base.h
index 9853d273826b3e6793b9fdf71ef058be9392eb46..534cf8537c54b3ec3b5d92e3435fa3c2532d9ada 100644
--- a/src/constraint_base.h
+++ b/src/constraint_base.h
@@ -66,11 +66,11 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
 
         /** \brief Evaluate the constraint given the input parameters and returning the residuals and jacobians
         **/
-        virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const = 0;
+        virtual bool evaluate(double const* const* _parameters, double* _residuals, double** _jacobians) const = 0;
 
         /** Returns a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr and the residual vector
          **/
-        virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const = 0;
+        virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& _residual, std::vector<Eigen::MatrixXs>& _jacobians) const = 0;
 
         /** \brief Returns the jacobians computation method
          **/
diff --git a/src/constraint_fix_bias.h b/src/constraint_fix_bias.h
index 407d368bb701e6db3a06c5e56a75740d067fad54..de1480919e5a1aa37d24c4a95b4114196f7796ba 100644
--- a/src/constraint_fix_bias.h
+++ b/src/constraint_fix_bias.h
@@ -3,11 +3,11 @@
 #define CONSTRAINT_FIX_BIAS_H_
 
 //Wolf includes
+#include <capture_IMU.h>
+#include <feature_IMU.h>
 #include "constraint_autodiff.h"
 #include "frame_base.h"
 #include "rotations.h"
-#include "capture_imu.h"
-#include "feature_imu.h"
 
 //#include "ceres/jet.h"
 
diff --git a/src/constraints/CMakeLists.txt b/src/constraints/CMakeLists.txt
index 7730d0f99921698ca23a90904fe99b08d8077bf9..e0f4ddfc41b1be8be8c4fb84ddaf96c6080dd952 100644
--- a/src/constraints/CMakeLists.txt
+++ b/src/constraints/CMakeLists.txt
@@ -3,6 +3,8 @@ INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
 # Forward var to parent scope
 
 SET(HDRS_CONSTRAINT ${HDRS_CONSTRAINT}
-                 ${CMAKE_CURRENT_SOURCE_DIR}/constraint_diff_drive.h PARENT_SCOPE)
+  ${CMAKE_CURRENT_SOURCE_DIR}/constraint_diff_drive.h 
+  PARENT_SCOPE)
 
-SET(SRCS_CONSTRAINT ${SRCS_CONSTRAINT} PARENT_SCOPE)
+SET(SRCS_CONSTRAINT ${SRCS_CONSTRAINT} 
+  PARENT_SCOPE)
diff --git a/src/examples/test_2_lasers_offline.cpp b/src/examples/test_2_lasers_offline.cpp
index b72c7f4c741e3b650e78fbd761ab983e8ce231ac..5df1a0f01a1c57fa6d0601fb77e9c18d2870344b 100644
--- a/src/examples/test_2_lasers_offline.cpp
+++ b/src/examples/test_2_lasers_offline.cpp
@@ -1,4 +1,5 @@
 //std includes
+#include <sensor_GPS_fix.h>
 #include <cstdlib>
 #include <iostream>
 #include <fstream>
@@ -21,7 +22,6 @@
 #include "../processor_odom_2D.h"
 #include "../sensor_laser_2D.h"
 #include "../sensor_odom_2D.h"
-#include "../sensor_gps_fix.h"
 #include "../ceres_wrapper/ceres_manager.h"
 
 // laserscanutils
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index 0498bca76cd95eaf464e8d265b3b2e46d8e6fc76..679777dd47ed49f034ffce24844e998ba3bd2a1a 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -1,4 +1,5 @@
 //std includes
+#include <sensor_GPS_fix.h>
 #include <cstdlib>
 #include <iostream>
 #include <fstream>
@@ -21,7 +22,6 @@
 #include "processor_odom_2D.h"
 #include "sensor_laser_2D.h"
 #include "sensor_odom_2D.h"
-#include "sensor_gps_fix.h"
 #include "ceres_wrapper/ceres_manager.h"
 
 // laserscanutils
diff --git a/src/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp
index ad821fffc9d3d426e0ce0a1f5a74726d50114c67..9668e1c4a2690d18d8c995e3963ebffbaf1478f3 100644
--- a/src/examples/test_ceres_2_lasers_polylines.cpp
+++ b/src/examples/test_ceres_2_lasers_polylines.cpp
@@ -1,4 +1,5 @@
 //std includes
+#include <sensor_GPS_fix.h>
 #include <cstdlib>
 #include <iostream>
 #include <fstream>
@@ -21,7 +22,6 @@
 #include "processor_odom_2D.h"
 #include "sensor_laser_2D.h"
 #include "sensor_odom_2D.h"
-#include "sensor_gps_fix.h"
 #include "ceres_wrapper/ceres_manager.h"
 
 // laserscanutils
diff --git a/src/examples/test_constraint_imu.cpp b/src/examples/test_constraint_imu.cpp
index b802e384efca7693df955196cb6a24035baa14b3..e4a3b8f2b37a5669bf10b32c16a2e8246bc9e52e 100644
--- a/src/examples/test_constraint_imu.cpp
+++ b/src/examples/test_constraint_imu.cpp
@@ -1,13 +1,13 @@
 //Wolf
+#include <capture_IMU.h>
+#include <processor_IMU.h>
+#include <sensor_IMU.h>
 #include "../capture_pose.h"
 #include "wolf.h"
 #include "problem.h"
-#include "sensor_imu.h"
-#include "capture_imu.h"
 #include "constraint_odom_3D.h"
 #include "state_block.h"
 #include "state_quaternion.h"
-#include "processor_imu.h"
 #include "ceres_wrapper/ceres_manager.h"
 
 //#define DEBUG_RESULTS
diff --git a/src/examples/test_imuDock.cpp b/src/examples/test_imuDock.cpp
index cc38459099d7b35ee5192b57a9e225312f4d82df..903e5c5201658143e776ba998180f7e5aaaffd2e 100644
--- a/src/examples/test_imuDock.cpp
+++ b/src/examples/test_imuDock.cpp
@@ -5,11 +5,11 @@
  *      \author: Dinesh Atchuthan
  */
 
+#include <processor_IMU.h>
+#include <sensor_IMU.h>
 #include "wolf.h"
 #include "problem.h"
 #include "ceres_wrapper/ceres_manager.h"
-#include "sensor_imu.h"
-#include "processor_imu.h"
 #include "sensor_odom_3D.h"
 #include "processor_odom_3D.h"
 
diff --git a/src/examples/test_imuDock_autoKFs.cpp b/src/examples/test_imuDock_autoKFs.cpp
index 660c6a240c3a491e7687241316a29f5349e8f405..53f2fd9bdc893a90698293d383760ac9b5c9621d 100644
--- a/src/examples/test_imuDock_autoKFs.cpp
+++ b/src/examples/test_imuDock_autoKFs.cpp
@@ -5,11 +5,11 @@
  *      \author: Dinesh Atchuthan
  */
 
+#include <processor_IMU.h>
+#include <sensor_IMU.h>
 #include "wolf.h"
 #include "problem.h"
 #include "ceres_wrapper/ceres_manager.h"
-#include "sensor_imu.h"
-#include "processor_imu.h"
 #include "sensor_odom_3D.h"
 #include "processor_odom_3D.h"
 
diff --git a/src/examples/test_imuPlateform_Offline.cpp b/src/examples/test_imuPlateform_Offline.cpp
index 0014cda5d57122247a3d18edbe532bc95b7a20ef..487fac3754c1960e6979cc19fdc87a209fea5e6f 100644
--- a/src/examples/test_imuPlateform_Offline.cpp
+++ b/src/examples/test_imuPlateform_Offline.cpp
@@ -1,13 +1,13 @@
 //Wolf
+#include <capture_IMU.h>
+#include <processor_IMU.h>
+#include <sensor_IMU.h>
 #include "wolf.h"
 #include "problem.h"
-#include "sensor_imu.h"
 #include "sensor_odom_3D.h"
-#include "capture_imu.h"
 #include "constraint_odom_3D.h"
 #include "state_block.h"
 #include "state_quaternion.h"
-#include "processor_imu.h"
 #include "processor_odom_3D.h"
 #include "ceres_wrapper/ceres_manager.h"
 
diff --git a/src/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp
index d56507525f98779c305df9b965fabb49654faad4..185406733ff2d9f5f13520d3a7de08e4c2539952 100644
--- a/src/examples/test_imu_constrained0.cpp
+++ b/src/examples/test_imu_constrained0.cpp
@@ -1,13 +1,13 @@
 //Wolf
+#include <capture_IMU.h>
+#include <processor_IMU.h>
+#include <sensor_IMU.h>
 #include "wolf.h"
 #include "problem.h"
-#include "sensor_imu.h"
 #include "sensor_odom_3D.h"
-#include "capture_imu.h"
 #include "constraint_odom_3D.h"
 #include "state_block.h"
 #include "state_quaternion.h"
-#include "processor_imu.h"
 #include "processor_odom_3D.h"
 #include "ceres_wrapper/ceres_manager.h"
 
diff --git a/src/examples/test_mpu.cpp b/src/examples/test_mpu.cpp
index da544d90afe9e0b14632fede31173cdd80fd49e4..4acbf55b7ad78818c5b16688471eee6126bd6828 100644
--- a/src/examples/test_mpu.cpp
+++ b/src/examples/test_mpu.cpp
@@ -6,15 +6,11 @@
  */
 
  //Wolf
+#include <capture_IMU.h>
 #include "wolf.h"
 #include "problem.h"
-#include "sensor_imu.h"
-#include "capture_imu.h"
 #include "state_block.h"
 #include "state_quaternion.h"
-#include "processor_imu.h"
-
-//std
 #include <iostream>
 #include <fstream>
 #include <iomanip>
@@ -22,6 +18,8 @@
 #include <cmath>
 #include <termios.h>
 #include <fcntl.h>
+#include <processor_IMU.h>
+#include <sensor_IMU.h>
 
 #define DEBUG_RESULTS
 #define FROM_FILE
diff --git a/src/examples/test_processor_imu.cpp b/src/examples/test_processor_imu.cpp
index 7cf885c573edf9b382a5062525f891c65cd1323c..119e2fe629e74a2e94815ffdb21234b5f9aa997f 100644
--- a/src/examples/test_processor_imu.cpp
+++ b/src/examples/test_processor_imu.cpp
@@ -6,15 +6,13 @@
  */
 
 //Wolf
+#include <capture_IMU.h>
+#include <processor_IMU.h>
+#include <sensor_IMU.h>
 #include "wolf.h"
 #include "problem.h"
-#include "sensor_imu.h"
-#include "capture_imu.h"
 #include "state_block.h"
 #include "state_quaternion.h"
-#include "processor_imu.h"
-
-//std
 #include <iostream>
 #include <fstream>
 #include <iomanip>
diff --git a/src/examples/test_processor_imu_jacobians.cpp b/src/examples/test_processor_imu_jacobians.cpp
index b01b753a549763a70b79b7f62243ef976d9a4517..6cdb01c976419b4ef10785c6e94bb4be7504bf90 100644
--- a/src/examples/test_processor_imu_jacobians.cpp
+++ b/src/examples/test_processor_imu_jacobians.cpp
@@ -6,15 +6,13 @@
  */
 
 //Wolf
+#include <capture_IMU.h>
+#include <sensor_IMU.h>
+#include <test/processor_IMU_UnitTester.h>
 #include "wolf.h"
 #include "problem.h"
-#include "sensor_imu.h"
-#include "capture_imu.h"
 #include "state_block.h"
 #include "state_quaternion.h"
-#include "processor_imu_UnitTester.h"
-
-//std
 #include <iostream>
 #include <fstream>
 #include <iomanip>
diff --git a/src/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp
index 4bc19cc404d69fdd5d506751a03cb14365a0e1b9..e01e30d75289eed205bbdefc2195f78c8c042e9f 100644
--- a/src/examples/test_processor_odom_3D.cpp
+++ b/src/examples/test_processor_odom_3D.cpp
@@ -6,10 +6,10 @@
  */
 
 
+#include <capture_IMU.h>
 #include "problem.h"
 #include "sensor_odom_2D.h"
 #include "processor_odom_3D.h"
-#include "capture_imu.h"
 #include "map_base.h"
 #include "landmark_base.h"
 #include "ceres_wrapper/ceres_manager.h"
diff --git a/src/examples/test_wolf_factories.cpp b/src/examples/test_wolf_factories.cpp
index c34616753a174a249f54cb5dbf75825ac4e8b843..8f7dfa4df1959516849056dc5c5251901a514c12 100644
--- a/src/examples/test_wolf_factories.cpp
+++ b/src/examples/test_wolf_factories.cpp
@@ -5,8 +5,9 @@
  *      \author: jsola
  */
 
+#include <processor_IMU.h>
+#include <sensor_GPS_fix.h>
 #include "../hardware_base.h"
-#include "../sensor_gps_fix.h"
 #include "../sensor_camera.h"
 #include "../sensor_odom_2D.h"
 #include "../sensor_imu.h"
@@ -14,8 +15,6 @@
 
 #include "../processor_odom_2D.h"
 #include "../processor_odom_3D.h"
-#include "../processor_imu.h"
-//#include "../processor_gps.h"
 #include "../processor_image_feature.h"
 
 #include "../problem.h"
diff --git a/src/feature_gps_fix.cpp b/src/feature_GPS_fix.cpp
similarity index 90%
rename from src/feature_gps_fix.cpp
rename to src/feature_GPS_fix.cpp
index 89a84180f68d22b5acbc401cf8e0cf557fccdcce..a6cac0e2fbb0591ef7facd954ce54367c8212571 100644
--- a/src/feature_gps_fix.cpp
+++ b/src/feature_GPS_fix.cpp
@@ -1,4 +1,4 @@
-#include "feature_gps_fix.h"
+#include <feature_GPS_fix.h>
 
 namespace wolf {
 
diff --git a/src/feature_gps_fix.h b/src/feature_GPS_fix.h
similarity index 95%
rename from src/feature_gps_fix.h
rename to src/feature_GPS_fix.h
index efddfbba967383889a0d9a7494d9246098a9df16..ea82b483efba8e76f0451f4891f434ddd304d8f9 100644
--- a/src/feature_gps_fix.h
+++ b/src/feature_GPS_fix.h
@@ -2,8 +2,8 @@
 #define FEATURE_GPS_FIX_H_
 
 //Wolf includes
+#include <constraint_GPS_2D.h>
 #include "feature_base.h"
-#include "constraint_gps_2D.h"
 
 //std includes
 
diff --git a/src/feature_gps_pseudorange.cpp b/src/feature_GPS_pseudorange.cpp
similarity index 96%
rename from src/feature_gps_pseudorange.cpp
rename to src/feature_GPS_pseudorange.cpp
index 347361788306a0826edccf53453038b1c6833283..2aa010a8573cc4c1920ba8a6c1231e46fcf437ce 100644
--- a/src/feature_gps_pseudorange.cpp
+++ b/src/feature_GPS_pseudorange.cpp
@@ -1,4 +1,4 @@
-#include "feature_gps_pseudorange.h"
+#include <feature_GPS_pseudorange.h>
 
 namespace wolf {
 
diff --git a/src/feature_gps_pseudorange.h b/src/feature_GPS_pseudorange.h
similarity index 100%
rename from src/feature_gps_pseudorange.h
rename to src/feature_GPS_pseudorange.h
diff --git a/src/feature_imu.cpp b/src/feature_IMU.cpp
similarity index 97%
rename from src/feature_imu.cpp
rename to src/feature_IMU.cpp
index b657039367682802ddfe7c285efa59e48a8c6b3a..c76b4378f139d9e1ef57cb0654ec449285a0223a 100644
--- a/src/feature_imu.cpp
+++ b/src/feature_IMU.cpp
@@ -1,4 +1,4 @@
-#include "feature_imu.h"
+#include <feature_IMU.h>
 
 namespace wolf {
 
diff --git a/src/feature_imu.h b/src/feature_IMU.h
similarity index 98%
rename from src/feature_imu.h
rename to src/feature_IMU.h
index 83a3e8c4dae7488f23e397a3f8e5de41a32eb87d..0f8d71fd9c511b2f9458809c1f07defd85a09983 100644
--- a/src/feature_imu.h
+++ b/src/feature_IMU.h
@@ -2,8 +2,8 @@
 #define FEATURE_IMU_H_
 
 //Wolf includes
+#include <capture_IMU.h>
 #include "feature_base.h"
-#include "capture_imu.h"
 #include "wolf.h"
 
 //std includes
diff --git a/src/features/CMakeLists.txt b/src/features/CMakeLists.txt
index d3cecc7f4d912bde79675ec88409bca8646939ec..ba959f48215e7310f605e8e57673b3dc64ef6af4 100644
--- a/src/features/CMakeLists.txt
+++ b/src/features/CMakeLists.txt
@@ -3,7 +3,9 @@ INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
 # Forward var to parent scope
 
 SET(HDRS_FEATURE ${HDRS_FEATURE}
-                 ${CMAKE_CURRENT_SOURCE_DIR}/feature_diff_drive.h PARENT_SCOPE)
+  ${CMAKE_CURRENT_SOURCE_DIR}/feature_diff_drive.h 
+  PARENT_SCOPE)
 
 SET(SRCS_FEATURE ${SRCS_FEATURE}
-                 ${CMAKE_CURRENT_SOURCE_DIR}/feature_diff_drive.cpp PARENT_SCOPE)
+  ${CMAKE_CURRENT_SOURCE_DIR}/feature_diff_drive.cpp 
+  PARENT_SCOPE)
diff --git a/src/landmark_point_3d.cpp b/src/landmark_point_3D.cpp
similarity index 93%
rename from src/landmark_point_3d.cpp
rename to src/landmark_point_3D.cpp
index 52c73f5f8d79edd98e0e20e0a3347762b24b6953..03c2aed8fcdb2563a0a02c5f0aed00722b61c39c 100644
--- a/src/landmark_point_3d.cpp
+++ b/src/landmark_point_3D.cpp
@@ -1,4 +1,4 @@
-#include "landmark_point_3d.h"
+#include <landmark_point_3D.h>
 
 namespace wolf {
 
diff --git a/src/landmark_point_3d.h b/src/landmark_point_3D.h
similarity index 100%
rename from src/landmark_point_3d.h
rename to src/landmark_point_3D.h
diff --git a/src/problem.cpp b/src/problem.cpp
index 609f85a25b484bef7b7104fcb62dbafe6dca4a2b..e36dc43c0828fc688a964905a7169c42e2a3afe0 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -1,3 +1,4 @@
+#include <sensor_GPS.h>
 #include "problem.h"
 #include "constraint_base.h"
 #include "state_block.h"
@@ -8,7 +9,6 @@
 #include "processor_motion.h"
 #include "processor_tracker.h"
 #include "sensor_base.h"
-#include "sensor_gps.h"
 #include "factory.h"
 #include "sensor_factory.h"
 #include "processor_factory.h"
diff --git a/src/processor_gps.cpp b/src/processor_GPS.cpp
similarity index 94%
rename from src/processor_gps.cpp
rename to src/processor_GPS.cpp
index 607057261cdb19c661dae7ff21434b744902e7ec..d66ba6e9b109060504987dfeb03cd30f77205102 100644
--- a/src/processor_gps.cpp
+++ b/src/processor_GPS.cpp
@@ -2,10 +2,11 @@
 // Created by ptirindelli on 16/12/15.
 //
 
-#include "processor_gps.h"
+#include <constraint_GPS_pseudorange_2D.h>
+#include <feature_GPS_pseudorange.h>
+#include <processor_GPS.h>
+
 #include "capture_gps.h"
-#include "feature_gps_pseudorange.h"
-#include "constraint_gps_pseudorange_2D.h"
 
 namespace wolf
 {
diff --git a/src/processor_gps.h b/src/processor_GPS.h
similarity index 100%
rename from src/processor_gps.h
rename to src/processor_GPS.h
diff --git a/src/processor_imu.cpp b/src/processor_IMU.cpp
similarity index 99%
rename from src/processor_imu.cpp
rename to src/processor_IMU.cpp
index 052e8ff8bb26345f1a422958fa4dd476ccf91b29..2e6340d05f75b96315f317bb7f17d29c80cb7121 100644
--- a/src/processor_imu.cpp
+++ b/src/processor_IMU.cpp
@@ -1,5 +1,5 @@
-#include "processor_imu.h"
-#include "imu_tools.h"
+#include <IMU_tools.h>
+#include <processor_IMU.h>
 
 namespace wolf {
 
diff --git a/src/processor_imu.h b/src/processor_IMU.h
similarity index 97%
rename from src/processor_imu.h
rename to src/processor_IMU.h
index 47decd316b1f420611629e390d783c63c64a337c..b0c77d6b903ee6399caef17f9d0897859ce44f4e 100644
--- a/src/processor_imu.h
+++ b/src/processor_IMU.h
@@ -2,9 +2,9 @@
 #define PROCESSOR_IMU_H
 
 // Wolf
+#include <capture_IMU.h>
+#include <feature_IMU.h>
 #include "processor_motion.h"
-#include "capture_imu.h"
-#include "feature_imu.h"
 
 
 namespace wolf {
@@ -103,10 +103,10 @@ class ProcessorIMU : public ProcessorMotion{
 /////////////////////////////////////////////////////////
 
 // Wolf
-#include "constraint_imu.h"
+#include <constraint_IMU.h>
 #include "state_block.h"
 #include "rotations.h"
-#include "imu_tools.h"
+#include <IMU_tools.h>
 
 
 namespace wolf{
diff --git a/src/processor_base.cpp b/src/processor_base.cpp
index 785cccc5e14383d41774d6ea322b0dd131d681a6..2cc289cdd55bec6a315936749db2d3cffe86cdb3 100644
--- a/src/processor_base.cpp
+++ b/src/processor_base.cpp
@@ -49,7 +49,6 @@ void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _
 {
     if (_keyframe_ptr != nullptr)
         kf_pack_buffer_.add(_keyframe_ptr,_time_tol_other);
-
 }
 
 void ProcessorBase::remove()
diff --git a/src/processor_base.h b/src/processor_base.h
index ce506925f2bb06118633912d70dfbfd44f5c9237..f9b33533bfa3dd85a0025ed481320c0cda493227 100644
--- a/src/processor_base.h
+++ b/src/processor_base.h
@@ -156,6 +156,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
          *   - Add the provided capture on it.
          */
         FrameBasePtr emplaceFrame(FrameType _type, CaptureBasePtr _capture_ptr);
+
         /**\brief make a Frame with the provided Capture
          *
          * Provide the following functionality:
@@ -166,8 +167,6 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
          */
         FrameBasePtr emplaceFrame(FrameType _type, CaptureBasePtr _capture_ptr, const Eigen::VectorXs& _state);
 
-//        virtual bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) = 0;
-
         void keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other);
 
         SensorBasePtr getSensorPtr();
diff --git a/src/processor_tracker.h b/src/processor_tracker.h
index 08304472ab9c433f89a27d064d17aeddd47c355b..d4c63b71f56091e9ec770fefaf0d99f7a7e32b4b 100644
--- a/src/processor_tracker.h
+++ b/src/processor_tracker.h
@@ -185,7 +185,7 @@ class ProcessorTracker : public ProcessorBase
         /**\brief Creates and adds constraints from last_ to origin_
          *
          */
-        virtual void establishConstraints()=0;
+        virtual void establishConstraints() = 0;
 
         /** \brief Reset the tracker using the \b last Capture as the new \b origin.
          */
diff --git a/src/processor_tracker_feature.h b/src/processor_tracker_feature.h
index 82253eefcd6291d9233dc9025de89ecc236a7d6b..302d5f41699c9345913f316f3cef0e5c0a4d7844 100644
--- a/src/processor_tracker_feature.h
+++ b/src/processor_tracker_feature.h
@@ -108,8 +108,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
          * \param _feature_list_out returned list of features found in \b incoming
          * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
          */
-        virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out,
-                                           FeatureMatchMap& _feature_correspondences) = 0;
+        virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, FeatureMatchMap& _feature_correspondences) = 0;
 
         /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
          * \param _origin_feature input feature in origin capture tracked
diff --git a/src/processors/CMakeLists.txt b/src/processors/CMakeLists.txt
index 8cc83297348ef677e37284c002f2cf1d199b99e3..5bcd585e623d58a3478a900a0667e4a7c8dfc48a 100644
--- a/src/processors/CMakeLists.txt
+++ b/src/processors/CMakeLists.txt
@@ -3,7 +3,10 @@ INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
 # Forward var to parent scope
 
 SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
-    ${CMAKE_CURRENT_SOURCE_DIR}/processor_diff_drive.h PARENT_SCOPE)
+  ${CMAKE_CURRENT_SOURCE_DIR}/processor_diff_drive.h 
+  PARENT_SCOPE)
 
 SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
-    ${CMAKE_CURRENT_SOURCE_DIR}/processor_diff_drive.cpp PARENT_SCOPE)
+  ${CMAKE_CURRENT_SOURCE_DIR}/processor_diff_drive.cpp 
+  PARENT_SCOPE)
+
diff --git a/src/sensor_gps.cpp b/src/sensor_GPS.cpp
similarity index 98%
rename from src/sensor_gps.cpp
rename to src/sensor_GPS.cpp
index 9e34f638544c87631d549944411ab73c6ec4affc..8d16b8691e5bf8a85e9e44f2e476f66bd46981c7 100644
--- a/src/sensor_gps.cpp
+++ b/src/sensor_GPS.cpp
@@ -1,5 +1,5 @@
 
-#include "sensor_gps.h"
+#include <sensor_GPS.h>
 #include "state_block.h"
 #include "state_quaternion.h"
 
diff --git a/src/sensor_gps.h b/src/sensor_GPS.h
similarity index 100%
rename from src/sensor_gps.h
rename to src/sensor_GPS.h
diff --git a/src/sensor_gps_fix.cpp b/src/sensor_GPS_fix.cpp
similarity index 97%
rename from src/sensor_gps_fix.cpp
rename to src/sensor_GPS_fix.cpp
index 6d21ac0346a8ebe36c43edbf54474b5a45a2675a..956e3fb687352c20d609617e0089b6f42e299be7 100644
--- a/src/sensor_gps_fix.cpp
+++ b/src/sensor_GPS_fix.cpp
@@ -1,4 +1,4 @@
-#include "sensor_gps_fix.h"
+#include <sensor_GPS_fix.h>
 #include "state_block.h"
 #include "state_quaternion.h"
 
diff --git a/src/sensor_gps_fix.h b/src/sensor_GPS_fix.h
similarity index 100%
rename from src/sensor_gps_fix.h
rename to src/sensor_GPS_fix.h
diff --git a/src/sensor_imu.cpp b/src/sensor_IMU.cpp
similarity index 98%
rename from src/sensor_imu.cpp
rename to src/sensor_IMU.cpp
index 972cc687fe1ace0a9560b1cc9faa601504f6cb00..6e973746eafa75f0642e646e6d1edec9e33a0a49 100644
--- a/src/sensor_imu.cpp
+++ b/src/sensor_IMU.cpp
@@ -1,5 +1,4 @@
-#include "sensor_imu.h"
-
+#include <sensor_IMU.h>
 #include "state_block.h"
 #include "state_quaternion.h"
 
diff --git a/src/sensor_imu.h b/src/sensor_IMU.h
similarity index 99%
rename from src/sensor_imu.h
rename to src/sensor_IMU.h
index 940278b6a1347030b31dafcb616d3b6f5e6ec3d5..823c18507efe6976f002d310f6edf9c89de2b4da 100644
--- a/src/sensor_imu.h
+++ b/src/sensor_IMU.h
@@ -62,7 +62,7 @@ class SensorIMU : public SensorBase
          * \param _a_w_biases_ptr StateBlock pointer to the vector of acc and gyro biases
          *
          **/
-        SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsIMUPtr params);
+        SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsIMUPtr _params);
 
         Scalar getGyroNoise() const;
         Scalar getAccelNoise() const;
diff --git a/src/sensors/CMakeLists.txt b/src/sensors/CMakeLists.txt
index 65520638ce8de1fd3e7200873e0b95f0b1ba22c3..5b634817ee7ef1b81e67cdf29af8ed8a227b131a 100644
--- a/src/sensors/CMakeLists.txt
+++ b/src/sensors/CMakeLists.txt
@@ -3,9 +3,11 @@ INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
 # Forward var to parent scope
 
 SET(HDRS_SENSOR ${HDRS_SENSOR}
-                ${CMAKE_CURRENT_SOURCE_DIR}/diff_drive_tools.h
-                ${CMAKE_CURRENT_SOURCE_DIR}/diff_drive_tools.hpp
-                ${CMAKE_CURRENT_SOURCE_DIR}/sensor_diff_drive.h PARENT_SCOPE)
+  ${CMAKE_CURRENT_SOURCE_DIR}/diff_drive_tools.h
+  ${CMAKE_CURRENT_SOURCE_DIR}/diff_drive_tools.hpp
+  ${CMAKE_CURRENT_SOURCE_DIR}/sensor_diff_drive.h 
+  PARENT_SCOPE)
 
 SET(SRCS_SENSOR ${SRCS_SENSOR}
-                ${CMAKE_CURRENT_SOURCE_DIR}/sensor_diff_drive.cpp PARENT_SCOPE)
+  ${CMAKE_CURRENT_SOURCE_DIR}/sensor_diff_drive.cpp 
+  PARENT_SCOPE)
diff --git a/src/solver/solver_manager.h b/src/solver/solver_manager.h
index 75d18f8f8cfa34c40b745ada2c97831362a99863..81a8e083eb30587543382814bae24f9662196335 100644
--- a/src/solver/solver_manager.h
+++ b/src/solver/solver_manager.h
@@ -2,13 +2,13 @@
 #define CERES_MANAGER_H_
 
 //wolf includes
+#include <constraint_GPS_2D.h>
 #include "../wolf.h"
 #include "../state_block.h"
 #include "../state_point.h"
 #include "../state_complex_angle.h"
 #include "../state_theta.h"
 #include "../constraint_sparse.h"
-#include "../constraint_gps_2D.h"
 #include "../constraint_odom_2D_theta.h"
 #include "../constraint_odom_2D_complex_angle.h"
 #include "../constraint_corner_2D_theta.h"
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index 27f02386aaa0f0a9489573f33b7b34a457882546..b0cf56188cebdd44124f96c9c4c02772bf98d403 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -61,8 +61,8 @@ wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
 target_link_libraries(gtest_frame_base ${PROJECT_NAME})
 
 # IMU tools test
-wolf_add_gtest(gtest_imu_tools gtest_imu_tools.cpp)
-# target_link_libraries(gtest_imu_tools ${PROJECT_NAME}) // WOLF library not needed
+wolf_add_gtest(gtest_IMU_tools gtest_IMU_tools.cpp)
+# target_link_libraries(gtest_IMU_tools ${PROJECT_NAME}) // WOLF library not needed
 
 # IMU tools test
 wolf_add_gtest(gtest_KF_pack_buffer gtest_KF_pack_buffer.cpp)
@@ -119,10 +119,10 @@ wolf_add_gtest(gtest_constraint_pose_3D gtest_constraint_pose_3D.cpp)
 target_link_libraries(gtest_constraint_pose_3D ${PROJECT_NAME})
 
 # FeatureIMU test
-wolf_add_gtest(gtest_feature_imu gtest_feature_imu.cpp)
-target_link_libraries(gtest_feature_imu ${PROJECT_NAME})
+wolf_add_gtest(gtest_feature_IMU gtest_feature_IMU.cpp)
+target_link_libraries(gtest_feature_IMU ${PROJECT_NAME})
 
-# IMU Bias + estimation tests
+# IMUTest (Bias + estimation)
 wolf_add_gtest(gtest_IMU gtest_IMU.cpp)
 target_link_libraries(gtest_IMU ${PROJECT_NAME})
 
@@ -135,12 +135,12 @@ wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2D gtest_processor_
 target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2D ${PROJECT_NAME})
 
 # ProcessorIMU class test
-wolf_add_gtest(gtest_processor_imu gtest_processor_imu.cpp)
-target_link_libraries(gtest_processor_imu ${PROJECT_NAME})
+wolf_add_gtest(gtest_processor_IMU gtest_processor_IMU.cpp)
+target_link_libraries(gtest_processor_IMU ${PROJECT_NAME})
 
-# ProcessorIMU_jacobians test
-wolf_add_gtest(gtest_processorIMU_jacobians gtest_processorIMU_jacobians.cpp)
-target_link_libraries(gtest_processorIMU_jacobians ${PROJECT_NAME})
+# ProcessorIMUJacobians test
+wolf_add_gtest(gtest_processor_IMU_jacobians gtest_processor_IMU_jacobians.cpp)
+target_link_libraries(gtest_processor_IMU_jacobians ${PROJECT_NAME})
 
 # ProcessorMotion in 2D
 wolf_add_gtest(gtest_odom_2D gtest_odom_2D.cpp)
diff --git a/src/test/gtest_IMU.cpp b/src/test/gtest_IMU.cpp
index 5c5f48204e39cb1e42f71517fb89cddcf948f561..b3862ed88e9e5289f293eac251a625ba3a622c0c 100644
--- a/src/test/gtest_IMU.cpp
+++ b/src/test/gtest_IMU.cpp
@@ -6,9 +6,9 @@
  */
 
 //Wolf
+#include <processor_IMU.h>
+#include <sensor_IMU.h>
 #include "wolf.h"
-#include "sensor_imu.h"
-#include "processor_imu.h"
 #include "sensor_odom_3D.h"
 #include "processor_odom_3D.h"
 #include "ceres_wrapper/ceres_manager.h"
diff --git a/src/test/gtest_imu_tools.cpp b/src/test/gtest_IMU_tools.cpp
similarity index 99%
rename from src/test/gtest_imu_tools.cpp
rename to src/test/gtest_IMU_tools.cpp
index 2e5563d305e1a132565f7002a1a374453f4a31bb..fa0420892dc49c09a4f30c1a950c4daa0fcfeddd 100644
--- a/src/test/gtest_imu_tools.cpp
+++ b/src/test/gtest_IMU_tools.cpp
@@ -5,8 +5,7 @@
  *      Author: jsola
  */
 
-#include "imu_tools.h"
-
+#include <IMU_tools.h>
 #include "utils_gtest.h"
 
 
diff --git a/src/test/gtest_constraint_imu.cpp b/src/test/gtest_constraint_IMU.cpp
similarity index 99%
rename from src/test/gtest_constraint_imu.cpp
rename to src/test/gtest_constraint_IMU.cpp
index efa1647b758ec88b6cff56ec62cf94b648d9cfd5..3accb792b20b23f5f7d5e6b31fbf0a7dfd7fd662 100644
--- a/src/test/gtest_constraint_imu.cpp
+++ b/src/test/gtest_constraint_IMU.cpp
@@ -6,9 +6,9 @@
  */
 
 //Wolf
-#include "sensor_imu.h"
-#include "capture_imu.h"
-#include "processor_imu.h"
+#include <capture_IMU.h>
+#include <processor_IMU.h>
+#include <sensor_IMU.h>
 #include "processor_odom_3D.h"
 #include "ceres_wrapper/ceres_manager.h"
 #include "utils_gtest.h"
diff --git a/src/test/gtest_feature_imu.cpp b/src/test/gtest_feature_IMU.cpp
similarity index 98%
rename from src/test/gtest_feature_imu.cpp
rename to src/test/gtest_feature_IMU.cpp
index 6609f97080de513b1d01ee73f749ff1685490332..5178fb63deec0af50a7e9a5ded8707c0cf44ba30 100644
--- a/src/test/gtest_feature_imu.cpp
+++ b/src/test/gtest_feature_IMU.cpp
@@ -1,12 +1,11 @@
 //Wolf
+#include <capture_IMU.h>
+#include <processor_IMU.h>
+#include <sensor_IMU.h>
 #include "wolf.h"
 #include "problem.h"
-#include "sensor_imu.h"
-#include "capture_imu.h"
 #include "state_block.h"
 #include "state_quaternion.h"
-#include "processor_imu.h"
-
 #include "utils_gtest.h"
 #include "../src/logging.h"
 
diff --git a/src/test/gtest_processor_imu.cpp b/src/test/gtest_processor_IMU.cpp
similarity index 99%
rename from src/test/gtest_processor_imu.cpp
rename to src/test/gtest_processor_IMU.cpp
index a3b57eb668f9285fb7ce619873a107fc0958b4c6..f8aac4c30823043472aa73614012bd55cedf4287 100644
--- a/src/test/gtest_processor_imu.cpp
+++ b/src/test/gtest_processor_IMU.cpp
@@ -5,15 +5,15 @@
  *      \author: jsola
  */
 
+#include <capture_IMU.h>
+#include <processor_IMU.h>
+#include <sensor_IMU.h>
 #include "wolf.h"
 
 #include "utils_gtest.h"
 #include "logging.h"
 
 #include "rotations.h"
-#include "sensor_imu.h"
-#include "processor_imu.h"
-#include "capture_imu.h"
 #include "ceres_wrapper/ceres_manager.h"
 
 #include <cmath>
diff --git a/src/test/gtest_processorIMU_jacobians.cpp b/src/test/gtest_processor_IMU_jacobians.cpp
similarity index 99%
rename from src/test/gtest_processorIMU_jacobians.cpp
rename to src/test/gtest_processor_IMU_jacobians.cpp
index 6761a4521e9c9132dc7b1a061af61200ebd0f97d..dc8677e169f6fa957915668176de2c6b195e377c 100644
--- a/src/test/gtest_processorIMU_jacobians.cpp
+++ b/src/test/gtest_processor_IMU_jacobians.cpp
@@ -6,15 +6,13 @@
  */
 
  //Wolf
+#include <capture_IMU.h>
+#include <sensor_IMU.h>
+#include <test/processor_IMU_UnitTester.h>
 #include "wolf.h"
 #include "problem.h"
-#include "sensor_imu.h"
-#include "capture_imu.h"
 #include "state_block.h"
 #include "state_quaternion.h"
-#include "processor_imu_UnitTester.h"
-
-//std
 #include <iostream>
 #include <fstream>
 #include <iomanip>
diff --git a/src/test/processor_imu_UnitTester.cpp b/src/test/processor_IMU_UnitTester.cpp
similarity index 82%
rename from src/test/processor_imu_UnitTester.cpp
rename to src/test/processor_IMU_UnitTester.cpp
index 4716356e6415f3ed3b803c6ebb68cdd3a8b5036e..a37a50ece9b0084df96b0bd2f214289c33ff27e2 100644
--- a/src/test/processor_imu_UnitTester.cpp
+++ b/src/test/processor_IMU_UnitTester.cpp
@@ -1,4 +1,4 @@
-#include "processor_imu_UnitTester.h"
+#include <test/processor_IMU_UnitTester.h>
 
 namespace wolf {
 
diff --git a/src/test/processor_imu_UnitTester.h b/src/test/processor_IMU_UnitTester.h
similarity index 99%
rename from src/test/processor_imu_UnitTester.h
rename to src/test/processor_IMU_UnitTester.h
index 2c70cec4ac2570229dc4169072e9ad9586829990..7e4c2124b7f35d162a458d2bbd310df55a39e46e 100644
--- a/src/test/processor_imu_UnitTester.h
+++ b/src/test/processor_IMU_UnitTester.h
@@ -3,7 +3,7 @@
 #define PROCESSOR_IMU_UNITTESTER_H
 
 // Wolf
-#include "processor_imu.h"
+#include <processor_IMU.h>
 #include "processor_motion.h"
 
 namespace wolf {
diff --git a/src/yaml/processor_imu_yaml.cpp b/src/yaml/processor_IMU_yaml.cpp
similarity index 98%
rename from src/yaml/processor_imu_yaml.cpp
rename to src/yaml/processor_IMU_yaml.cpp
index 8ac801dd2da793cbd5a0f3e14efcd14502931d50..4b04af6253f3852b889e031f7e2c1ded71bb1856 100644
--- a/src/yaml/processor_imu_yaml.cpp
+++ b/src/yaml/processor_IMU_yaml.cpp
@@ -6,10 +6,10 @@
  */
 
 // wolf yaml
+#include <processor_IMU.h>
 #include "yaml_conversion.h"
 
 // wolf
-#include "../processor_imu.h"
 #include "../factory.h"
 
 // yaml-cpp library
diff --git a/src/yaml/sensor_imu_yaml.cpp b/src/yaml/sensor_IMU_yaml.cpp
similarity index 98%
rename from src/yaml/sensor_imu_yaml.cpp
rename to src/yaml/sensor_IMU_yaml.cpp
index babce6140ffdb9046448fa0120b94f0cc72b364d..6a68bdc7ea507b111617cdfb29eb5259944b5f65 100644
--- a/src/yaml/sensor_imu_yaml.cpp
+++ b/src/yaml/sensor_IMU_yaml.cpp
@@ -6,10 +6,10 @@
  */
 
 // wolf yaml
+#include <sensor_IMU.h>
 #include "yaml_conversion.h"
 
 // wolf
-#include "../sensor_imu.h"
 #include "../factory.h"
 
 // yaml-cpp library
diff --git a/wolf_scripts/ReadMe.md b/wolf_scripts/ReadMe.md
new file mode 100644
index 0000000000000000000000000000000000000000..0237e751b1cf4adb968ed013e7e0ded01b32cd37
--- /dev/null
+++ b/wolf_scripts/ReadMe.md
@@ -0,0 +1,35 @@
+# SCRIPTS for DEVELOPERS
+
+Helpful scripts to create WOLF tree elements (e.g. processors)
+
+## Installation
+
+#### Dependencies
+
+  * __RealPath__ (Required): Install with the following command.
+  
+  ..*`sudo apt-get install realpath`
+  
+#### SCRIPTS installation  
+
+  * Move to the scripts folder
+  
+  ..*`cd $WOLF_ROOT/wolf_scripts`  
+
+  * Run the following line to set your `bashrc`. This will allow you to execute the scripts from any console path.
+  
+  ..*`echo "source $WOLF_ROOT/wolf_scripts/setup.bash" >> ~/.bashrc`
+
+## Usage
+
+#### Create a new derived class
+
+Creates the main structure including .cpp and .h files, and a gtest for the new derived class (all in corresponding folders of WOLF directory. 
+
+  * Example of usage  
+  `create_wolf_processor.sh -t processor -n example -b tracker` 
+
+  * Options:
+    - t: Type. Any of the following [ capture | constraint | feature | processor | sensor ]"
+    - n: Name 
+    - b: Base class (inherited from this class)
\ No newline at end of file
diff --git a/wolf_scripts/generic_func/functions.sh b/wolf_scripts/generic_func/functions.sh
new file mode 100755
index 0000000000000000000000000000000000000000..57da3bbfdd1b3f7f22e0695576743fd1513b4352
--- /dev/null
+++ b/wolf_scripts/generic_func/functions.sh
@@ -0,0 +1,518 @@
+#! /bin/bash
+
+# ===== FUNCTIONS =====
+
+# Bash colors
+RED=$(tput setaf 1)
+GREEN=$(tput setaf 2)
+YELLOW=$(tput setaf 3)
+BLUE=$(tput setaf 4)
+CYAN=$(tput setaf 6)
+NC=$(tput sgr 0) # No Color
+
+askIfGitBranch()
+{
+ read -p "${CYAN}Do you want to create a new git branch(Y/n)?  ${NC}" ANSWER
+ DOIT=${ANSWER:-y}
+ OK=0
+ 
+ while [ $OK -eq 0 ];
+ do
+   case "$ANSWER" in
+     "y" | "Y")
+ 	   DOIT="y"
+ 	   OK=1
+ 	   ;;
+     "n" | "N")
+       DOIT="n"
+       OK=1
+       ;; 
+     *)
+       OK=0
+       echo "${RED}  [ERROR]: Incorrect answer.${NC} Please type y or n"
+       exit
+       ;;
+  esac
+done   
+
+if [ "$DOIT" == "y" ];
+then
+  if [ -z "$1" ]
+  then
+    echo "${RED}  [ERROR]: No argument supplied.${NC}"
+  fi
+  cd $WOLF_ROOT 
+  git checkout -b $1
+  echo "${GREEN} \--> Created git branch $1${NC}"
+fi
+echo ""    
+}
+
+getEnvVariable()
+{
+  local var=`echo "${!1}"`
+  if [ -z "${var}" ]
+  then
+    echo "${RED}  [ERROR]: The environment variable ${1} has not been defined.${NC} Please see the wiki documentation for instructions on how to create it." >&2
+    exit
+  fi
+  echo $var
+}
+
+showHelp() 
+{
+  echo "******************************************"  
+  echo "${BLUE}Script to generate a WOLF $1 class${NC}"
+  echo "******************************************"
+  echo " Required parameters:"
+  echo "  -${CYAN}t${NC}: type. Any of the following [ ${CYAN}capture${NC} | ${CYAN}constraint${NC} | ${CYAN}feature${NC} | ${CYAN}processor${NC} | ${CYAN}sensor${NC}]"
+  echo "  -${CYAN}n${NC}: name"
+  echo "  -${CYAN}b${NC}: base class (inheritance)"
+  echo ""  
+  echo "Example of usage:"
+  echo "wolf_create.sh -t processor -n processor_example -b processor_tracker" 
+  echo ""
+}
+
+LowerCase()
+{
+	if [ $1 ]
+	then
+  		echo $1 | tr '[:upper:]' '[:lower:]'
+	else
+  		echo "${RED}  [ERROR]: No $1 provided, aborting ...${NC}"
+  		exit
+	fi
+}
+
+UpperCase()
+{
+	if [ $1 ]
+	then
+  		echo $1 | tr '[:lower:]' '[:upper:]'
+	else
+  		echo "${RED}  [ERROR]: No $1 provided, aborting ...${NC}"
+  		exit
+	fi
+}
+
+UpperCaseFirstLetter()
+{
+  echo "$(echo "$1" | sed 's/.*/\u&/')"
+}
+
+capitalizeDiminutives()
+{
+  RES=$1
+  if [[ $1 =~ .*"2d"*. ]] ;  
+  then
+    RES=$(echo "${1/2d/2D}")
+  fi
+  if [[ $1 =~ .*"3d"*. ]] ;
+  then
+    RES=$(echo "${RES/3d/3D}")
+  fi
+  if [[ $1 =~ .*"gps"*. ]] ;
+  then
+    RES=$(echo "${RES/gps/GPS}")
+  fi  
+  if [[ $1 =~ .*"imu"*. ]] ;
+  then
+    RES=$(echo "${RES/imu/IMU}")    
+  fi
+  echo $RES
+}
+
+getFilePath()
+{
+  if find $WOLF_ROOT -name "${1}" -print -quit | grep -q '^'; 
+  then
+    echo "$(find "$WOLF_ROOT/src" -name ${1} )"
+  else
+    echo ""
+  fi
+}
+
+fillWithBaseConstructorParameters()
+{
+  # Header: class constructor with base class parameters	
+  H_TXT=$(cat $BASE_H_PATH)
+  H_TXT="${H_TXT#*class $BASECLASSNAME :}"
+  BASECLASS_TXT="${H_TXT%%\n\};*}"
+  PARAMS="${BASECLASS_TXT#*$BASECLASSNAME(}"
+
+  BASE_DERIVES_FROM_AUTODIFF=""
+  if [[ $PARAMS =~ .*ConstraintAutodiff*. ]] && ! [[ $BASECLASSNAME =~ .*ConstraintAutodiff*. ]] ;
+  then
+    BASE_DERIVES_FROM_AUTODIFF="TRUE"
+  fi    
+
+  PARAMS="${PARAMS%%\) :*}" # in case of inheritance
+  PARAMS="${PARAMS%%\);*}"
+
+  if [[ "$PARAMS" == *"const std::string& _type"* ]] ;
+  then
+  	PARAMS="${PARAMS#*_type,}"
+   	NAME_STR="${NAME#${TYPE}_}"
+   	NAME_STR=$(UpperCase $NAME_STR)
+  fi
+
+  OLD=" class_name();"
+  NEW="\ \ \ \ \ \ \ \ ${CLASSNAME}(${PARAMS});"
+  NEW=${NEW//$'\n'/} # Remove all newlines.
+  NEW=${NEW%$'\n'}   # Remove a trailing newline.
+  NEW=$(echo $NEW | sed 's/^ *//g' | sed 's/ *$//g') # remove extra whitespaces
+  sed '/'"${OLD}"'/c'"${NEW}"'' "${TEMPLATES_PATH}"/class_template.h > "${TEMPLATES_PATH}"/tmp.h
+
+  # CPP: class constructor with base class parameters	
+  PARAMS="$PARAMS," # add , at the end to ease things
+  PARAMS=$(echo "$PARAMS" | sed 's/\ =.*,\ /,\ /g')
+  PARAMS=$(echo "$PARAMS" | sed 's/\ =.*,/,/g')    
+  PARAMS="${PARAMS::-1}" # remove , from the end
+
+  OLD="class_name::class_name() :"
+  NEW="class_name::class_name(${PARAMS}) :"
+  NEW=${NEW//$'\n'/} # Remove all newlines.
+  NEW=${NEW%$'\n'}   # Remove a trailing newline.
+  NEW=$(echo $NEW | sed 's/^ *//g' | sed 's/ *$//g') # remove extra whitespaces
+  sed '/'"${OLD}"'/c'"${NEW}"'' "${TEMPLATES_PATH}"/class_template.cpp > "${TEMPLATES_PATH}"/tmp.cpp           
+
+  # get only the variable names
+  PARAMS_CLEAN=$(echo $PARAMS | sed -r 's/'","'/ /g')
+  ar=($PARAMS_CLEAN)
+  PARAMS_OBJ=
+  for el in "${ar[@]}"; do
+    if [[ $el == _* ]] 
+    then
+      PARAMS_OBJ+=", ${el}"
+    fi
+  done
+  PARAMS_OBJ=${PARAMS_OBJ#","}
+  if ! [[ -z $NAME_STR ]] ;
+  then
+    PARAMS_OBJ="\"$NAME_STR\", $PARAMS_OBJ"
+  fi
+  OLD="\        base_class()"
+  NEW="\        base_class(${PARAMS_OBJ} )"
+  sed '/'"${OLD}"'/c'"${NEW}"'' "${TEMPLATES_PATH}"/tmp.cpp > "${TEMPLATES_PATH}"/tmp2.cpp 
+  rm "${TEMPLATES_PATH}"/tmp.cpp
+  mv "${TEMPLATES_PATH}"/tmp2.cpp "${TEMPLATES_PATH}"/tmp.cpp
+}
+
+createHCPPFromTemplates()
+{
+  # Templates path
+  TEMPLATES_PATH=${WOLF_SCRIPTS_PATH}/templates
+
+  # ===== Create HEADER and CPP files =====
+
+  # Pick initialization parameters from base class
+  fillWithBaseConstructorParameters
+
+  #Set the TYPE and class names on the template files
+  sed 's/header_file/'"${NAME}.h"'/g' "${TEMPLATES_PATH}"/tmp.cpp > "${TEMPLATES_PATH}"/tmp2.cpp
+  sed 's/class_name/'"${CLASSNAME}"'/g' "${TEMPLATES_PATH}"/tmp2.cpp > "${TEMPLATES_PATH}"/tmp3.cpp
+  sed 's/base_class/'"${BASECLASSNAME}"'/g' "${TEMPLATES_PATH}"/tmp3.cpp > "${TEMPLATES_PATH}"/tmp4.cpp
+  rm "${TEMPLATES_PATH}"/tmp.cpp
+  rm "${TEMPLATES_PATH}"/tmp2.cpp
+  rm "${TEMPLATES_PATH}"/tmp3.cpp
+ 
+  sed 's/base_header_file/'"${BASE}.h"'/g' "${TEMPLATES_PATH}"/tmp.h > "${TEMPLATES_PATH}"/tmp2.h
+  sed 's/name_cap/'"${TYPE_CAP}_${BASE_CAP}_${NAME_CAP}"'/g' "${TEMPLATES_PATH}"/tmp2.h > "${TEMPLATES_PATH}"/tmp3.h
+  sed 's/class_name/'"${CLASSNAME}"'/g' "${TEMPLATES_PATH}"/tmp3.h > "${TEMPLATES_PATH}"/tmp4.h
+  sed 's/base_class/'"${BASECLASSNAME}"'/g' "${TEMPLATES_PATH}"/tmp4.h > "${TEMPLATES_PATH}"/tmp5.h
+  rm "${TEMPLATES_PATH}"/tmp.h
+  rm "${TEMPLATES_PATH}"/tmp2.h
+  rm "${TEMPLATES_PATH}"/tmp3.h
+  rm "${TEMPLATES_PATH}"/tmp4.h
+  
+  # Rename and move files
+  NAME_H_PATH="$WOLF_ROOT"/src/"$TYPE"s/"$NAME".h
+  NAME_CPP_PATH="$WOLF_ROOT"/src/"$TYPE"s/"$NAME".cpp
+  mv "${TEMPLATES_PATH}"/tmp5.h "$NAME_H_PATH"
+  mv "${TEMPLATES_PATH}"/tmp4.cpp "$NAME_CPP_PATH"
+}
+
+addAutodiffSpecifics()
+{
+  # Number of parameters
+  echo ""
+  echo "${CYAN}To create the class $CLASSNAME, you have to provide some info:${NC}"
+  echo "" 
+
+  PROMPT="${CYAN}- What is the size (dimensions) of the residual?${NC} (1 integer, followed by [ENTER]):"
+  read -p "${PROMPT}" RESIDUAL_DIM; 
+  if ! [[ "$RESIDUAL_DIM" =~ ^[0-9]+$ ]] ;
+  then
+    echo "${RED}  [ERROR]: Invalid input.${NC} Expecting a numeric value. Aborting..."
+    exit 1.
+  fi
+  echo -en "\033[1A\033[2K"
+  echo -en "\033[1A\033[2K"
+  echo "${GREEN} \--> Setting residual size to $RESIDUAL_DIM.${NC}"
+  echo ""
+  PROMPT="${CYAN}- How many state blocks are going to be considered in the constraint?${NC} (1 integer, followed by [ENTER]):"
+  read -p "${PROMPT}" NUM_STATES; 
+  if ! [[ "$NUM_STATES" =~ ^[0-9]+$ ]] ;
+  then
+    echo "${RED}  [ERROR]: Invalid input.${NC} Expecting a numeric value. Aborting..."
+    exit 1.
+  fi
+  echo -en "\033[1A\033[2K"
+  echo -en "\033[1A\033[2K"
+  echo "${GREEN} \--> Setting $NUM_STATES state blocks.${NC}"
+  echo ""
+  
+  for (( idx = 0; idx < $NUM_STATES; idx++ )); do
+     PROMPT="${CYAN}- Name of state $((idx+1))?${NC} (1 string, followed by [ENTER]):"
+     read -p "${PROMPT}" STATENAME;
+     PROMPT="${CYAN}- Size (dimensions) of state $((idx+1)) (${STATENAME})?${NC} (1 integer, followed by [ENTER]:"
+     read -p "${PROMPT}" STATESIZE; 
+     if ! [[ "$STATESIZE" =~ ^[0-9]+$ ]] ;
+     then
+       echo "${RED}  [ERROR]: Invalid input.${NC} Expecting a numeric value. Aborting..."
+       exit 1.
+     fi
+     echo -en "\033[1A\033[2K"
+     echo -en "\033[1A\033[2K"
+     NAMES+=( "${STATENAME}" )
+     SIZES+=( "${STATESIZE}" )
+  done
+
+  echo -en "\033[1A\033[2K"
+  echo -n "${GREEN} \--> Setting state blocks: ${NC}"
+  PARAMS=
+  for (( idx = 0; idx < ${#NAMES[@]}; idx++ )); do
+    echo -n "${GREEN}${NAMES[$idx]}(${SIZES[$idx]}) ${NC}"
+    PARAMS+=( "const T* const _${NAMES[idx]},")
+    PARAM_NUMS+=( "${SIZES[idx]},")
+  done
+  echo ""
+  
+  PARAMS[-1]=${PARAMS[-1]%?}
+  PARAM_NUMS[-1]=${PARAM_NUMS[-1]%?}
+
+  sed -i "s/public $BASECLASSNAME/public $BASECLASSNAME<$CLASSNAME, $RESIDUAL_DIM, ${PARAM_NUMS[*]}>/g" "$NAME_H_PATH"
+  sed -i "/virtual \~$CLASSNAME/a\ \n\        \/\*\* \brief : compute the residual from the state blocks being iterated by the solver.\n \        \*\*\/\n\        template<typename T>\n\        bool operator ()(${PARAMS[*]}, const T* const _residuals) const;\n" "$NAME_H_PATH"
+  sed -i "/\} \/\/ namespace wolf/a\ \n\/\/ Include here all headers for this class\n\/\/\#include <YOUR_HEADERS.h>\n\nnamespace wolf\n\{\n\ntemplate<typename T> bool $CLASSNAME::operator ()(${PARAMS[*]}, const T* const _residuals) const\n\{\n  std::cout << \"\\033[1;33m [WARN]:\\033[0m ${CLASSNAME}::operator () is empty.\" << std::endl;\n  \/\/ TODO: Implement\n  return true;\n\}\n\n\} \/\/ namespace wolf" "$NAME_H_PATH"
+}
+
+fillWithBaseVirtualMethods()
+{
+  if ! [ -z $BASE_DERIVES_FROM_AUTODIFF ] ;
+  then
+    echo "${YELLOW} [ WARN]: Base class $BASECLASSNAME derives from AUTODIFF template. New .h and .cpp files are left without inherited functions."
+  else
+  
+    if [[ $BASECLASSNAME =~ .*ConstraintAutodiff*. ]] ;
+    then
+      addAutodiffSpecifics
+    else
+      # Get base class	
+      H_TXT=$(cat $BASE_H_PATH)
+      H_TXT="${H_TXT##*class $BASECLASSNAME :}"
+      BASECLASS_TXT="${H_TXT%%\n\};*}"
+
+      echo "class $BASECLASSNAME :$BASECLASS_TXT };" > "${WOLF_SCRIPTS_PATH}"/class.h
+
+      # H file Get Virtual function declarations with help
+      sed -e '/./{H;$!d;}' -e 'x;/ = 0;/!d;' "${WOLF_SCRIPTS_PATH}"/class.h > "${WOLF_SCRIPTS_PATH}"/tmp.h
+      sed -r 's/'" = 0"'//g' "${WOLF_SCRIPTS_PATH}"/tmp.h > "${WOLF_SCRIPTS_PATH}"/tmp2.h
+      sed -i -e "/virtual \~$CLASSNAME/r ${WOLF_SCRIPTS_PATH}/tmp2.h" "$NAME_H_PATH" 
+      rm "${WOLF_SCRIPTS_PATH}"/tmp.h	
+      rm ${WOLF_SCRIPTS_PATH}/tmp2.h
+    
+      # CPP file  
+      FuncInBase=$(grep -e " = 0;" -e "=0;" "${WOLF_SCRIPTS_PATH}/class.h")
+      rm "${WOLF_SCRIPTS_PATH}"/class.h
+    
+      D=";"   #Multi Character Delimiter
+      FuncList=($(echo $FuncInBase | sed -e 's/'"$D"'/\n/g' | while read line; do echo $line | sed 's/[\t ]/'"$D"'/g'; done))
+
+      for (( idx = $((${#FuncList[@]}-1)); idx > -1; idx-- )); do
+  	    TMP=$(echo ${FuncList[idx]} | sed -r 's/'"$D"'/ /g')
+        TMP=$(echo $TMP | sed -r 's/'"virtual"'/ /g')
+        TMP=$(echo $TMP | sed -r 's/'"=0"'/ /g')
+        TMP=$(echo $TMP | sed -r 's/'" = 0"'/ /g')
+        TXTH=${TMP%$TMP##*[![:space:]]}
+        TXTCPP_3=$(echo $TXTH | sed 's/.*(//g')
+        TXTCPP_2=$(echo $TXTH | sed 's/(.*//g' | sed 's/.* //g')
+        TXTCPP_1=$(echo $TXTH | sed 's/'"$TXTCPP_2"'.*//g')
+ 
+        # CPP file
+        TXTCPP_3=$(echo "$TXTCPP_3" | sed -r 's/\*\*+/XXXX/g') ## remove **
+        TXTCPP_3=$(echo "$TXTCPP_3" | sed -r 's/\*+/YYYY/g') ## remove *
+        sed -i "/\} \/\/ namespace wolf/i ${TXTCPP_1}${CLASSNAME}::${TXTCPP_2}(${TXTCPP_3}" "$NAME_CPP_PATH"
+        if ! [[ $TXTCPP_1 =~ .*void*. ]]
+        then
+          sed -i "/${CLASSNAME}::${TXTCPP_2}(${TXTCPP_3}/a \{\n  std::cout << \"\\033[1;33m [WARN]:\\033[0m ${CLASSNAME}::${TXTCPP_2} is empty.\" << std::endl;\n  ${TXTCPP_1}return_var\{\}; \/\/TODO: fill this variable\n  return return_var;\n\}\n" "$NAME_CPP_PATH"
+        else 
+          sed -i "/${CLASSNAME}::${TXTCPP_2}(${TXTCPP_3}/a \{\n  std::cout << \"\\033[1;33m [WARN]:\\033[0m ${CLASSNAME}::${TXTCPP_2} is empty.\" << std::endl;\n\}\n" "$NAME_CPP_PATH"
+        fi
+        sed -i -r 's/'"XXXX"'/'"**"'/g' "$NAME_CPP_PATH" # add again **
+        sed -i -r 's/'"YYYY"'/'"*"'/g' "$NAME_CPP_PATH" # add again *
+      done    
+    fi
+  fi  
+}
+
+updateCMakeLists()
+{	
+  CML_PATH="${WOLF_ROOT}/src/${TYPE}s/CMakeLists.txt"
+
+  # Add Header source
+  Hsources=( $(grep -e ".h" "${CML_PATH}") )
+  NewH="\${CMAKE_CURRENT_SOURCE_DIR}/$NAME.h"
+  
+  # Check if already exists
+  EXISTS=0
+  for (( idx = 0; idx < ${#Hsources[@]}; idx++ )); do
+   if [ "${Hsources[idx]}" == "\${CMAKE_CURRENT_SOURCE_DIR}/"$NAME".h" ] ;
+   then
+     EXISTS=1
+   fi
+  done
+
+  if [ $EXISTS == 0 ] ;
+  then
+    Hsources=( "${Hsources[@]}" $NewH )
+    IFS=$'\n' 
+    sorted=($(sort <<<"${Hsources[*]}"))
+    unset IFS
+    SET_AFTER_POS=-2
+     
+    for (( idx = 0; idx < ${#sorted[@]}; idx++ )); do
+      if [ "${sorted[idx]}" == "\${CMAKE_CURRENT_SOURCE_DIR}/"$NAME".h" ] ;
+      then
+  	    SET_AFTER_POS=$(( idx-1 ))
+  	    if [ $SET_AFTER_POS == -1 ] ; 
+  	    then
+  	      if [ ${#sorted[@]} == 1 ] ;
+  	      then 
+  	        sed -i "\%HDRS_CONSTRAINT%a \  \${CMAKE_CURRENT_SOURCE_DIR}/$NAME.h" "$CML_PATH"
+  	      else
+    	    sed -i "\%${sorted[1]}%i \  \${CMAKE_CURRENT_SOURCE_DIR}/$NAME.h" "$CML_PATH"
+    	  fi
+  	    else
+  	  	  sed -i "\%${sorted[$SET_AFTER_POS]}%a \  \${CMAKE_CURRENT_SOURCE_DIR}/$NAME.h" "$CML_PATH"
+  	    fi
+      fi	  	
+    done
+  
+    if ! [[ $BASECLASSNAME =~ .*ConstraintAutodiff*. ]] ;
+    then
+      # Add CPP source
+      Hsources=( $(grep -e ".cpp" "${CML_PATH}") )
+      NewCPP="\${CMAKE_CURRENT_SOURCE_DIR}/$NAME.cpp"
+      Hsources=( "${Hsources[@]}" $NewCPP )
+      IFS=$'\n' 
+      sorted=($(sort <<<"${Hsources[*]}"))
+      unset IFS
+      SET_AFTER_POS=-2
+      
+      for (( idx = 0; idx < ${#sorted[@]}; idx++ )); do
+        if [ "${sorted[idx]}" == "\${CMAKE_CURRENT_SOURCE_DIR}/"$NAME".cpp" ] ;
+        then
+          SET_AFTER_POS=$(( idx-1 ))
+  	      if [ $SET_AFTER_POS == -1 ] ; 
+  	      then
+  	        if [ ${#sorted[@]} == 1 ] ;
+  	        then 
+  	          sed -i "\%SRCS_CONSTRAINT%a \  \${CMAKE_CURRENT_SOURCE_DIR}/$NAME.cpp" "$CML_PATH"
+  	        else
+    	      sed -i "\%${sorted[1]}%i \  \${CMAKE_CURRENT_SOURCE_DIR}/$NAME.cpp" "$CML_PATH"
+    	    fi
+  	      else
+  	  	    sed -i "\%${sorted[$SET_AFTER_POS]}%a \  \${CMAKE_CURRENT_SOURCE_DIR}/$NAME.cpp" "$CML_PATH"
+  	      fi
+        fi	  	
+      done
+    fi  
+    echo "$CML_PATH"
+  else
+    echo ""
+  fi
+}
+
+createGtest()
+{
+  # ===== Create gtest file =====
+  # Templates path
+  TEMPLATES_PATH=${WOLF_SCRIPTS_PATH}/templates
+
+  # Set the include and class names on the template files
+  sed 's/header_file/'"${TYPE}s\/${NAME}.h"'/g' "${TEMPLATES_PATH}"/gtest_template.cpp > "${TEMPLATES_PATH}"/tmp.cpp
+  sed 's/class_name/'"${CLASSNAME}"'/g' "${TEMPLATES_PATH}"/tmp.cpp > "${TEMPLATES_PATH}"/tmp2.cpp
+  rm "${TEMPLATES_PATH}"/tmp.cpp
+
+  FUNC_NAMES=
+  # Insert dummy tests for all methods
+  for (( idx = $((${#FuncList[@]}-1)); idx > -1; idx-- )); do
+    TMP=$(echo ${FuncList[idx]} | sed -r 's/'"$D"'/ /g')
+    TMP=$(echo $TMP | sed -r 's/'"virtual"'/ /g')
+    TMP=$(echo $TMP | sed -r 's/'"=0"'/ /g')
+    TMP=$(echo $TMP | sed -r 's/'" = 0"'/ /g')
+    TMP=${TMP%$TMP##*[![:space:]]}
+    TMP=$(echo $TMP | sed 's/(.*//g' | sed 's/.* //g')
+    if ! grep -q "$TMP" "${TEMPLATES_PATH}"/tmp2.cpp
+    then
+      sed -i "/\[Class methods\]/a TEST($CLASSNAME, $TMP)\n\{\n  std::cout << \"\\033[1;33m [WARN]:\\033[0m gtest for ${CLASSNAME} ${TMP} is empty.\" << std::endl;\n\}\n" "${TEMPLATES_PATH}"/tmp2.cpp
+    fi
+  done
+
+  GTEST_PATH="${WOLF_ROOT}"/src/test/gtest_${NAME}.cpp
+  mv "${TEMPLATES_PATH}"/tmp2.cpp $GTEST_PATH
+}
+
+updateCMakeListsGTest()
+{
+  CML_GTEST_PATH="${WOLF_ROOT}/src/test/CMakeLists.txt"
+
+  DERIVED_TXT=$(cat $CML_GTEST_PATH  | sed '/^#/!d')
+  DERIVED_TXT="${DERIVED_TXT##*\# ------- Now Derived classes ----------}"
+  DERIVED_TXT="${DERIVED_TXT%%\# ------- Now Core classes Serialization ----------*}"
+  DERIVED_TXT=$(echo "$DERIVED_TXT" | sed '/^\s*$/d')
+  IFS=$'\n'
+  DERIVED_TXT=($(echo "$DERIVED_TXT" | sed -r 's/'"#\s"'/ /g'))
+  for (( idx = 0; idx < ${#DERIVED_TXT[@]}; idx++ )); do
+    DERIVED_TXT[idx]=${DERIVED_TXT[idx]#" "}
+    DERIVED_TXT[idx]=$(echo ${DERIVED_TXT[idx]} | sed 's/ .*//')
+  done
+
+  New="${CLASSNAME}"
+
+  # Check if already exists
+  EXISTS=0
+  for (( idx = 0; idx < ${#DERIVED_TXT[@]}; idx++ )); do
+   if [ "${DERIVED_TXT[idx]}" == "$New" ] ;
+   then
+     EXISTS=1
+   fi
+  done
+  
+  if [ $EXISTS == 0 ] ;
+  then
+    DERIVED_TXT=( "${DERIVED_TXT[@]}" $New )
+    IFS=$'\n' 
+    sorted=($(sort <<<"${DERIVED_TXT[*]}"))
+    unset IFS
+    SET_BEFORE_POS=-2
+    for (( idx = 0; idx < ${#sorted[@]}; idx++ )); do
+      if [ "${sorted[idx]}" == "$New" ] ;
+      then
+        SET_BEFORE_POS=$(( idx+1 ))
+        if [ $SET_BEFORE_POS == ${#sorted[@]} ] ; 
+        then
+          SET_BEFORE_POS=$(( idx ))
+        fi
+      fi	  	
+    done
+    sed -i "\%${sorted[$SET_BEFORE_POS]}%i # $New test\nwolf_add_gtest(gtest_$NAME gtest_$NAME.cpp)\ntarget_link_libraries(gtest_$NAME \$\{PROJECT_NAME\})\n" "${CML_GTEST_PATH}"
+    echo "$CML_GTEST_PATH"
+  else
+    echo ""
+  fi
+}
+
+# ============================
+# ============================
diff --git a/wolf_scripts/generic_func/user_menu.sh b/wolf_scripts/generic_func/user_menu.sh
new file mode 100755
index 0000000000000000000000000000000000000000..c472afa3e9f0fead8818071dc6167cc402209d55
--- /dev/null
+++ b/wolf_scripts/generic_func/user_menu.sh
@@ -0,0 +1,94 @@
+NAME=
+BASE=
+TYPE=
+while getopts ":t:n:b:" OPTION
+do
+  case ${OPTION} in
+    t)
+       TYPE=${OPTARG}
+       ;;
+    n)
+       NAME=${OPTARG}
+       ;;
+    b)
+       BASE=${OPTARG}
+       ;;
+    *) 
+       showHelp 
+       exit
+       ;;
+  esac
+done
+if [ -z "$TYPE" ] && [ -z "$NAME" ] && [ -z "$BASE" ]
+then
+   showHelp	
+   exit
+fi
+if [ -z "$TYPE" ] ;
+then
+   echo ""
+   echo "${RED}  [ERROR]: Missing TYPE value (-t).${NC}"
+   echo ""
+   showHelp	
+   exit
+fi
+
+if [ -z "$NAME" ] ;
+then
+   echo ""
+   echo "${RED}  [ERROR]: Missing NAME value (-n).${NC}"
+   echo ""
+   showHelp	
+   exit
+fi
+if [ -z "$BASE" ] ;
+then
+   echo ""
+   echo "${RED}  [ERROR]: Missing BASE value (-b).${NC}"
+   echo ""
+   showHelp	
+   exit
+fi
+
+if ! [ "$TYPE" == "capture" ] && ! [ "$TYPE" == "constraint" ] && ! [ "$TYPE" == "feature" ] && ! [ "$TYPE" == "processor" ] && ! [ "$TYPE" == "sensor" ]
+then
+   echo ""
+   echo "${RED} --> Incorrect type \"$TYPE\". Please check the following instructions: ${NC}"
+   echo ""
+   showHelp
+   exit 1
+fi
+
+NAME=$(LowerCase $NAME)
+NAME=$(capitalizeDiminutives $NAME)
+NAME_CAP=$(UpperCase $NAME)
+NAME_CAP1=$(UpperCaseFirstLetter $NAME)
+
+BASE=$(LowerCase $BASE)
+BASE=$(capitalizeDiminutives $BASE)
+BASE_CAP=$(UpperCase $BASE)
+BASE_CAP1=$(UpperCaseFirstLetter $BASE)
+
+TYPE=$(LowerCase $TYPE)
+TYPE_CAP=$(UpperCase $TYPE)
+TYPE_CAP1=$(UpperCaseFirstLetter $TYPE)
+
+CLASSNAME="$TYPE_CAP1$BASE_CAP1$NAME_CAP1"
+CLASSNAME=$(echo ${CLASSNAME} | sed 's/_\(.\)/\U\1/g') # remove "_" and capitalize next character
+
+BASECLASSNAME=$(echo ${BASE} | sed 's/_\(.\)/\U\1/g') # remove "_" and capitalize next character
+BASECLASSNAME=$TYPE_CAP1$(echo "$(echo "$BASECLASSNAME" | sed 's/.*/\u&/')") 
+
+if ! [[ $NAME =~ $TYPE ]] ;
+then
+  if [[ $BASE =~ .*base*. ]] ;
+  then
+    NAME="$TYPE"_"$NAME";  
+  else
+    NAME="$TYPE"_"$BASE"_"$NAME";
+  fi
+fi
+if ! [[ $BASE =~ $TYPE ]] ;
+then
+    BASE="$TYPE"_"$BASE";
+fi
diff --git a/wolf_scripts/setup.bash b/wolf_scripts/setup.bash
new file mode 100755
index 0000000000000000000000000000000000000000..5f767f034d7d6839e9c32f93ad78cc3fbdb4371e
--- /dev/null
+++ b/wolf_scripts/setup.bash
@@ -0,0 +1,6 @@
+#!/bin/bash
+
+WOLF_SCRIPTS_PATH=`realpath "${BASH_SOURCE[0]}"`
+
+export WOLF_SCRIPTS_PATH=${WOLF_SCRIPTS_PATH%/*}
+export PATH=${PATH}:${WOLF_SCRIPTS_PATH}
diff --git a/wolf_scripts/templates/class_template.cpp b/wolf_scripts/templates/class_template.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d33d26f0ed62425a580339c63760551db8f8920d
--- /dev/null
+++ b/wolf_scripts/templates/class_template.cpp
@@ -0,0 +1,18 @@
+#include "header_file"
+ 
+namespace wolf {
+
+// Constructor
+// TODO Modify this default API to suit your class needs
+class_name::class_name() :
+        base_class()
+{
+    // TODO Add your code or remove this comment
+}
+
+// Destructor
+class_name::~class_name()
+{
+}
+
+} // namespace wolf
diff --git a/wolf_scripts/templates/class_template.h b/wolf_scripts/templates/class_template.h
new file mode 100644
index 0000000000000000000000000000000000000000..79158895d26a8cb3b02ccb3694c9687b595ae125
--- /dev/null
+++ b/wolf_scripts/templates/class_template.h
@@ -0,0 +1,29 @@
+#ifndef _name_cap_H_
+#define _name_cap_H_
+
+//Wolf includes
+#include "wolf.h"
+#include "base_header_file"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(class_name);
+
+class class_name : public base_class
+{
+    public:
+
+        /** \brief Class constructor
+         */
+        // TODO Modify this default API to suit your class needs
+        class_name();
+
+        /** \brief Class Destructor
+         */
+        virtual ~class_name();
+};
+
+} // namespace wolf
+
+#endif /* _name_cap_H_ */
diff --git a/wolf_scripts/templates/gtest_template.cpp b/wolf_scripts/templates/gtest_template.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6dcb079c6dc5ae20a7c6d6667f7d80ace722f232
--- /dev/null
+++ b/wolf_scripts/templates/gtest_template.cpp
@@ -0,0 +1,37 @@
+#include "utils_gtest.h"
+
+#include "wolf.h"
+#include "logging.h"
+
+#include "header_file"
+
+using namespace Eigen;
+using namespace wolf;
+using std::static_pointer_cast;
+
+// Use the following in case you want to initialize tests with predefines variables or methods.
+//class class_name_class : public testing::Test{
+//    public:
+//        virtual void SetUp()
+//        {
+//        }
+//};
+
+TEST(class_name, Constructor)
+{
+  std::cout << "\033[1;33m [WARN]:\033[0m gtest for class_name Constructor is empty." << std::endl;
+}
+
+TEST(class_name, Destructor)
+{
+  std::cout << "\033[1;33m [WARN]:\033[0m gtest for class_name Destructor is empty." << std::endl;
+}
+
+//[Class methods]
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}
+
diff --git a/wolf_scripts/wolf_create.sh b/wolf_scripts/wolf_create.sh
new file mode 100755
index 0000000000000000000000000000000000000000..caa95ec03ee55f33da3409daef37a1598b498db1
--- /dev/null
+++ b/wolf_scripts/wolf_create.sh
@@ -0,0 +1,92 @@
+#! /bin/bash
+
+# Load functions
+. $WOLF_SCRIPTS_PATH/generic_func/functions.sh
+
+# check environment variables are been defined
+WOLF_ROOT=$(getEnvVariable WOLF_ROOT)
+WOLF_SCRIPTS_PATH=$(getEnvVariable WOLF_SCRIPTS_PATH)
+if [ -z "${WOLF_ROOT}" ] || [ -z "${WOLF_SCRIPTS_PATH}" ] ;
+then 
+	exit 
+fi
+
+# Source user menu to obtain main variables
+. $WOLF_SCRIPTS_PATH/generic_func/user_menu.sh
+
+#============================================
+echo ""
+echo "==========================================================================================================================="
+echo "${BLUE}     Creating new class and methods for $NAME derived from $BASE ${NC}"
+echo "==========================================================================================================================="
+echo ""
+#============================================
+
+# Create git branch is requested
+askIfGitBranch $NAME
+
+#============================================
+echo "- Generating CPP and H files."
+#============================================
+
+# Find base class files
+BASE_H_PATH=$(getFilePath $BASE.h)
+if [ -z "$BASE_H_PATH" ]
+then
+  echo ""
+  echo "${RED}  [ERROR]: Cannot find header file for base class ${BASE}.${NC}"
+  echo ""
+  exit
+fi
+
+# Create Header and CPP files
+createHCPPFromTemplates $NAME_H_PATH $NAME_CPP_PATH
+
+# Copy all pure virtual methods from base class to derived class
+fillWithBaseVirtualMethods
+
+echo "${GREEN} \--> Created $NAME_H_PATH file.${NC}"
+echo "${GREEN} \--> Created $NAME_CPP_PATH file.${NC}"
+#============================================
+
+# ===== Modify CMakeLists.txt =====
+echo ""
+echo "- Modifying CMakeLists.txt to include CPP and H files."
+
+DONE=$(updateCMakeLists)
+if [ -z "$DONE" ]
+then
+  echo "${YELLOW} \--x [WARN]: Not necessary. File entries already existing in CMakeLists.txt.${NC}"
+else
+  echo "${GREEN} \--> Updated ${DONE} file.${NC}"
+fi
+
+# ===== Create gtest =====
+echo ""
+echo "- Creating gtest for $NAME."
+
+createGtest
+
+echo "${GREEN} \--> Created ${GTEST_PATH} file.${NC}"
+
+#============================================
+
+# ===== Modify gtest CMakeLists.txt =====
+
+echo ""
+echo "- Modifying CMakeLists.txt to include gtest files."
+
+DONE=$(updateCMakeListsGTest)
+if [ -z "$DONE" ]
+then
+  echo "${YELLOW} \--x [WARN]: Not necessary. gtest file entry already existing in CMakeLists.txt.${NC}"
+else
+  echo "${GREEN} \--> Updated ${DONE} file.${NC}"
+fi
+
+#============================================
+
+echo ""
+echo "All Done."
+echo ""
+#============================================
\ No newline at end of file