diff --git a/.gitignore b/.gitignore
index f9bde2ac1e651b78ee2cebf56990d73edd8ba8fa..0b503d13489037739b59df5cbdb4f2881f417c72 100644
--- a/.gitignore
+++ b/.gitignore
@@ -5,7 +5,6 @@
 bin/
 build*/
 lib/
-gnss.found
 .vscode
 .vscode/c_cpp_properties.json
 /CMakeCache.txt
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 550b68aab10a2994361eadd34267574db7315ddd..f6e2ba23ed1e793e8072ff66bfa71531f5637010 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -105,7 +105,7 @@ stages:
   - cd build
   - cmake -DCMAKE_BUILD_TYPE=release-DBUILD_TESTS=ON ..
   - make -j$(nproc)
-  - ctest -j$(nproc)
+  - ctest -j$(nproc) --output-on-failure
   - make install
 
 .build_and_test_template: &build_and_test_definition
@@ -114,7 +114,7 @@ stages:
   - cd build
   - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_TESTS=ON ..
   - make -j$(nproc)
-  - ctest -j$(nproc)
+  - ctest -j$(nproc) --output-on-failure
   - make install
 
 ############ LICENSE HEADERS ############
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5e33300bc632a1f718f45f107f7d354fbe15bc0b..f3030d47ebc43bc959b318fea80ab97d570f965d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,10 +1,6 @@
 # Pre-requisites about cmake itself
 CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
 
-if(COMMAND cmake_policy)
-  cmake_policy(SET CMP0005 NEW)
-  cmake_policy(SET CMP0003 NEW)
-endif(COMMAND cmake_policy)
 # MAC OSX RPATH
 SET(CMAKE_MACOSX_RPATH 1)
 
@@ -16,7 +12,7 @@ MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
 
 SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
 SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
-set(INCLUDE_INSTALL_DIR include/iri-algorithms/wolf)
+set(INCLUDE_INSTALL_DIR include/wolf)
 set(LIB_INSTALL_DIR lib/)
 
 IF (NOT CMAKE_BUILD_TYPE)
@@ -33,7 +29,7 @@ include(CheckCXXCompilerFlag)
 CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
 if(COMPILER_SUPPORTS_CXX14)
 		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++14 support.")
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+    set(CMAKE_CXX_STANDARD 14)
 else()
   message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
 endif()
@@ -96,43 +92,39 @@ IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
   message(FATAL_ERROR "Bug: Specified CONFIG_DIR: "
     "${WOLF_CONFIG_DIR} exists, but is not a directory.")
 ENDIF()
+
 # Configure config.h
 configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
-message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
-include_directories("${PROJECT_BINARY_DIR}/conf")
-
-# ============ INCLUDES ============ 
-INCLUDE_DIRECTORIES(BEFORE "include")
 
 # ============ HEADERS ============ 
 SET(HDRS_CAPTURE
-  include/gnss/capture/capture_gnss.h
-  include/gnss/capture/capture_gnss_fix.h
-  include/gnss/capture/capture_gnss_tdcp.h
+  include/${PROJECT_NAME}/capture/capture_gnss.h
+  include/${PROJECT_NAME}/capture/capture_gnss_fix.h
+  include/${PROJECT_NAME}/capture/capture_gnss_tdcp.h
   )
 SET(HDRS_FACTOR
-  include/gnss/factor/factor_gnss_fix_2d.h
-  include/gnss/factor/factor_gnss_fix_3d.h
-  include/gnss/factor/factor_gnss_pseudo_range.h
-  include/gnss/factor/factor_gnss_tdcp.h
-  include/gnss/factor/factor_gnss_tdcp_2d.h
-  include/gnss/factor/factor_gnss_tdcp_3d.h
+  include/${PROJECT_NAME}/factor/factor_gnss_fix_2d.h
+  include/${PROJECT_NAME}/factor/factor_gnss_fix_3d.h
+  include/${PROJECT_NAME}/factor/factor_gnss_pseudo_range.h
+  include/${PROJECT_NAME}/factor/factor_gnss_tdcp.h
+  include/${PROJECT_NAME}/factor/factor_gnss_tdcp_2d.h
+  include/${PROJECT_NAME}/factor/factor_gnss_tdcp_3d.h
   )
 SET(HDRS_FEATURE
-  include/gnss/feature/feature_gnss_fix.h
-  include/gnss/feature/feature_gnss_tdcp.h
-  include/gnss/feature/feature_gnss_satellite.h
+  include/${PROJECT_NAME}/feature/feature_gnss_fix.h
+  include/${PROJECT_NAME}/feature/feature_gnss_tdcp.h
+  include/${PROJECT_NAME}/feature/feature_gnss_satellite.h
   )
 SET(HDRS_PROCESSOR
-  include/gnss/processor/processor_gnss_fix.h
-  include/gnss/processor/processor_gnss_tdcp.h
-  include/gnss/processor/processor_tracker_gnss.h
+  include/${PROJECT_NAME}/processor/processor_gnss_fix.h
+  include/${PROJECT_NAME}/processor/processor_gnss_tdcp.h
+  include/${PROJECT_NAME}/processor/processor_tracker_gnss.h
   )
 SET(HDRS_SENSOR
-  include/gnss/sensor/sensor_gnss.h
+  include/${PROJECT_NAME}/sensor/sensor_gnss.h
   )
 SET(HDRS_TREE_MANAGER
-  include/gnss/tree_manager/tree_manager_sliding_window_tdcp.h
+  include/${PROJECT_NAME}/tree_manager/tree_manager_sliding_window_tdcp.h
   )
 
 # ============ SOURCES ============ 
@@ -210,9 +202,9 @@ install(
   ${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
 )
 
-# Specifies include directories to use when compiling the plugin target
-# This way, include_directories does not need to be called in plugins depending on this one
-target_include_directories(${PLUGIN_NAME} INTERFACE
+target_include_directories(${PLUGIN_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<BUILD_INTERFACE:${PROJECT_BINARY_DIR}/conf>
   $<INSTALL_INTERFACE:${INCLUDE_INSTALL_DIR}>
 )
 
@@ -233,8 +225,6 @@ INSTALL(FILES ${HDRS_TREE_MANAGER}
 INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
   DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/internal)
 
-INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
-
 export(PACKAGE ${PLUGIN_NAME})
 
 FIND_PACKAGE(Doxygen MODULE)
diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h
index fece2683601328edd6d6f86b0efc5ba64ce36dc8..f0f1489059fa1a6e3a0e8d3cc420c2219c7276c4 100644
--- a/include/gnss/sensor/sensor_gnss.h
+++ b/include/gnss/sensor/sensor_gnss.h
@@ -52,6 +52,7 @@ struct ParamsSensorGnss : public ParamsSensorBase
         bool yaw_fixed = true;
         bool translation_fixed = true;
         Eigen::Vector3d ENU_latlonalt = Eigen::Vector3d::Zero();
+        bool latlon_in_degrees = false;
 
 
         ~ParamsSensorGnss() override = default;
@@ -71,7 +72,10 @@ struct ParamsSensorGnss : public ParamsSensorBase
                 yaw_fixed          = _server.getParam<bool>(prefix + _unique_name + "/ENU/yaw_fixed");
                 translation_fixed  = _server.getParam<bool>(prefix + _unique_name + "/ENU/translation_fixed");
                 if (ENU_mode == "manual")
-                    ENU_latlonalt      = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/ENU_latlonalt");
+                {
+                    ENU_latlonalt      = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/ENU/latlonalt");
+                    latlon_in_degrees  = _server.getParam<bool>(prefix + _unique_name + "/ENU/latlon_in_degrees");
+                }
             }
             else if (ENU_mode != "ECEF")
             {
diff --git a/src/capture/capture_gnss.cpp b/src/capture/capture_gnss.cpp
index 299f515a98a55a32ba9df65afff2c877e050c5d9..751561dfb0c6b15a38338f61ba82d1717b12b994 100644
--- a/src/capture/capture_gnss.cpp
+++ b/src/capture/capture_gnss.cpp
@@ -20,7 +20,7 @@
 //
 //--------LICENSE_END--------
 #include "gnss/capture/capture_gnss.h"
-
+#include <core/state_block/state_block_derived.h>
 
 namespace wolf {
 
@@ -30,18 +30,18 @@ CaptureGnss::CaptureGnss(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, GnssUt
 {
     // Clock bias
     assert(_sensor_ptr->getStateBlock('T') != nullptr and _sensor_ptr->isStateBlockDynamic('T'));
-    addStateBlock('T', std::make_shared<StateBlock>(1,true), nullptr);
+    addStateBlock('T', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
 
     // interconstellation clock bias
     assert(_sensor_ptr->getStateBlock('G') != nullptr);
     if(_sensor_ptr->isStateBlockDynamic('G'))
-        addStateBlock('G', std::make_shared<StateBlock>(1,true), nullptr);
+        addStateBlock('G', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
     assert(_sensor_ptr->getStateBlock('E') != nullptr);
     if(_sensor_ptr->isStateBlockDynamic('E'))
-        addStateBlock('E', std::make_shared<StateBlock>(1,true), nullptr);
+        addStateBlock('E', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
     assert(_sensor_ptr->getStateBlock('M') != nullptr);
     if(_sensor_ptr->isStateBlockDynamic('M'))
-        addStateBlock('M', std::make_shared<StateBlock>(1,true), nullptr);
+        addStateBlock('M', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
 
 }
 
diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp
index 6e8d73c399cbe429846a339386a821bc77c648e7..6ae5a98afc6c312e908f59d99e6d47401dbdaa54 100644
--- a/src/sensor/sensor_gnss.cpp
+++ b/src/sensor/sensor_gnss.cpp
@@ -20,7 +20,7 @@
 //
 //--------LICENSE_END--------
 #include "gnss/sensor/sensor_gnss.h"
-#include "core/state_block/state_block.h"
+#include <core/state_block/state_block_derived.h>
 #include "core/state_block/state_angle.h"
 #include "gnss_utils/gnss_utils.h"
 #include "gnss_utils/utils/transformations.h"
@@ -31,7 +31,7 @@ SensorGnss::SensorGnss(const Eigen::VectorXd& _extrinsics,
                        const ParamsSensorGnssPtr& _params)
         :
         SensorBase("SensorGnss",
-                   std::make_shared<StateBlock>(_extrinsics,_params->extrinsics_fixed),
+                   std::make_shared<StatePoint3d>(_extrinsics,_params->extrinsics_fixed),
                    nullptr, // antena orientation has no sense in GNSS
                    nullptr, // SensorParams are set afterwards
                    0),
@@ -46,15 +46,15 @@ SensorGnss::SensorGnss(const Eigen::VectorXd& _extrinsics,
 
     // STATE BLOCKS
     // ENU-MAP
-    addStateBlock('t', std::make_shared<StateBlock>(3, params_->translation_fixed), false);
+    addStateBlock('t', std::make_shared<StatePoint3d>(Vector3d::Zero(), params_->translation_fixed), false);
     addStateBlock('r', std::make_shared<StateAngle>(0.0, params_->roll_fixed), false);
     addStateBlock('p', std::make_shared<StateAngle>(0.0, params_->pitch_fixed), false);
     addStateBlock('y', std::make_shared<StateAngle>(0.0, params_->yaw_fixed), false);
     // clock bias
-    addStateBlock(CLOCK_BIAS_KEY, std::make_shared<StateBlock>(1,false), true); // receiver clock bias
-    addStateBlock(CLOCK_BIAS_GPS_GLO_KEY, std::make_shared<StateBlock>(1,true), params_->clock_bias_GPS_GLO_dynamic); // GPS-GLO clock bias
-    addStateBlock(CLOCK_BIAS_GPS_GAL_KEY, std::make_shared<StateBlock>(1,true), params_->clock_bias_GPS_GAL_dynamic); // GPS-GAL clock bias
-    addStateBlock(CLOCK_BIAS_GPS_CMP_KEY, std::make_shared<StateBlock>(1,true), params_->clock_bias_GPS_CMP_dynamic); // GPS-CMP clock bias
+    addStateBlock(CLOCK_BIAS_KEY, std::make_shared<StateParams1>(Vector1d::Zero(),false), true); // receiver clock bias
+    addStateBlock(CLOCK_BIAS_GPS_GLO_KEY, std::make_shared<StateParams1>(Vector1d::Zero(),true), params_->clock_bias_GPS_GLO_dynamic); // GPS-GLO clock bias
+    addStateBlock(CLOCK_BIAS_GPS_GAL_KEY, std::make_shared<StateParams1>(Vector1d::Zero(),true), params_->clock_bias_GPS_GAL_dynamic); // GPS-GAL clock bias
+    addStateBlock(CLOCK_BIAS_GPS_CMP_KEY, std::make_shared<StateParams1>(Vector1d::Zero(),true), params_->clock_bias_GPS_CMP_dynamic); // GPS-CMP clock bias
 
     // ENU-ECEF
     // Mode "manual": ENU provided via params
@@ -99,7 +99,9 @@ void SensorGnss::setEcefEnu(const Eigen::Vector3d& _ENU, bool _ECEF_coordinates)
     if (_ECEF_coordinates)
         GnssUtils::computeEnuEcefFromEcef(_ENU, R_ENU_ECEF_, t_ENU_ECEF_);
     else
-        GnssUtils::computeEnuEcefFromEcef(GnssUtils::latLonAltToEcef(_ENU), R_ENU_ECEF_, t_ENU_ECEF_);
+        GnssUtils::computeEnuEcefFromEcef(GnssUtils::latLonAltToEcef(_ENU, not params_->latlon_in_degrees), 
+                                          R_ENU_ECEF_, 
+                                          t_ENU_ECEF_);
 
     ENU_defined_ = true;