diff --git a/.gitignore b/.gitignore
index 4af768a7d9d0328dbaf8cbe79d9c82168a7a5ffa..f9bde2ac1e651b78ee2cebf56990d73edd8ba8fa 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,9 +2,11 @@
 .settings/language.settings.xml
 .project
 .cproject
-build/
 bin/
+build*/
 lib/
 gnss.found
 .vscode
 .vscode/c_cpp_properties.json
+/CMakeCache.txt
+/CMakeFiles/cmake.check_cache
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 71b2d3037f741c8670e9ad5b1ad14c8bc8ed92b5..550b68aab10a2994361eadd34267574db7315ddd 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -3,6 +3,13 @@ stages:
   - build_and_test
 
 ############ YAML ANCHORS ############
+.print_variables_template: &print_variables_definition
+  # Print variables
+  - echo $WOLF_CORE_BRANCH
+  - echo $CI_COMMIT_BRANCH
+  - echo $WOLF_GNSS_BRANCH
+  - echo $GNSSUTILS_BRANCH
+
 .preliminaries_template: &preliminaries_definition
   ## Install ssh-agent if not already installed, it is required by Docker.
   ## (change apt-get to yum if you use an RPM-based image)
@@ -85,9 +92,12 @@ stages:
   - if [ -d gnss_utils ]; then
   -   echo "directory gnss_utils exists"
   -   cd gnss_utils
+  -   git checkout main
+  -   git pull
+  -   git checkout ${GNSSUTILS_BRANCH}
   -   git pull
   - else
-  -   git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/gauss_project/gnss_utils.git
+  -   git clone -b ${GNSSUTILS_BRANCH} ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/gauss_project/gnss_utils.git
   -   cd gnss_utils
   -   git submodule update --init
   - fi
@@ -116,6 +126,7 @@ license_headers:
       paths:
       - ci_deps/wolf/
   before_script:
+    - *print_variables_definition
     - *preliminaries_definition
     - *install_wolf_definition
   script:
@@ -133,6 +144,7 @@ build_and_test:bionic:
       paths:
       - ci_deps/gnss_utils/
   before_script:
+    - *print_variables_definition
     - *preliminaries_definition
     - *install_wolf_definition
     - *install_gnssutils_definition
@@ -152,6 +164,7 @@ build_and_test:focal:
       paths:
       - ci_deps/gnss_utils/
   before_script:
+    - *print_variables_definition
     - *preliminaries_definition
     - *install_wolf_definition
     - *install_gnssutils_definition
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 89e8ce8771d7987d3a40afbd1f7a78a6f8aecc7a..5e33300bc632a1f718f45f107f7d354fbe15bc0b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,5 @@
 # Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
 
 if(COMMAND cmake_policy)
   cmake_policy(SET CMP0005 NEW)
@@ -16,7 +16,8 @@ MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
 
 SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
 SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
-SET(CMAKE_INSTALL_PREFIX /usr/local)
+set(INCLUDE_INSTALL_DIR include/iri-algorithms/wolf)
+set(LIB_INSTALL_DIR lib/)
 
 IF (NOT CMAKE_BUILD_TYPE)
   SET(CMAKE_BUILD_TYPE "DEBUG")
@@ -67,10 +68,6 @@ if(BUILD_TESTS)
     enable_testing()
 endif()
 
-#CMAKE modules
-SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
-MESSAGE(STATUS ${CMAKE_MODULE_PATH})
-
 # Some wolf compilation options
 IF((CMAKE_BUILD_TYPE MATCHES DEBUG) OR (CMAKE_BUILD_TYPE MATCHES debug) OR (CMAKE_BUILD_TYPE MATCHES Debug))
   set(_WOLF_DEBUG true)
@@ -79,12 +76,13 @@ ENDIF()
 option(_WOLF_TRACE "Enable wolf tracing macro" ON)
 
 # ============ DEPENDENCIES ============ 
-FIND_PACKAGE(wolfcore REQUIRED)
+FIND_PACKAGE(wolfcore REQUIRED CONFIG)
 FIND_PACKAGE(gnss_utils REQUIRED)
 
 # ============ CONFIG.H ============ 
-string(TOUPPER ${PROJECT_NAME} UPPER_NAME)
 set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
+# variable used to compile the config.h.in file
+string(TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
 
 # Define the directory where will be the configured config.h
 SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/${PROJECT_NAME}/internal)
@@ -104,8 +102,6 @@ message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
 include_directories("${PROJECT_BINARY_DIR}/conf")
 
 # ============ INCLUDES ============ 
-INCLUDE_DIRECTORIES(${wolfcore_INCLUDE_DIRS})
-INCLUDE_DIRECTORIES(${gnss_utils_INCLUDE_DIRS})
 INCLUDE_DIRECTORIES(BEFORE "include")
 
 # ============ HEADERS ============ 
@@ -118,7 +114,6 @@ 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_single_diff_2d.h
   include/gnss/factor/factor_gnss_tdcp.h
   include/gnss/factor/factor_gnss_tdcp_2d.h
   include/gnss/factor/factor_gnss_tdcp_3d.h
@@ -180,8 +175,8 @@ endif()
 
 #Link the created libraries
 #===============EXAMPLE=========================
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfcore_LIBRARIES})
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${gnss_utils_LIBRARIES})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} wolfcore)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} gnss_utils)
 
 #Build tests
 #===============EXAMPLE=========================
@@ -193,38 +188,56 @@ ENDIF(BUILD_TESTS)
 #install library
 #=============================================================
 INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
-      RUNTIME DESTINATION bin
-      LIBRARY DESTINATION lib
-      ARCHIVE DESTINATION lib)
+  RUNTIME DESTINATION bin
+  LIBRARY DESTINATION ${LIB_INSTALL_DIR}
+  ARCHIVE DESTINATION ${LIB_INSTALL_DIR}
+)
+install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/${PLUGIN_NAME}/cmake)
+
+# Configure the package installation
+include(CMakePackageConfigHelpers)
+configure_package_config_file(
+  ${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake.in
+  ${CMAKE_CURRENT_BINARY_DIR}/${PLUGIN_NAME}Config.cmake
+  INSTALL_DESTINATION ${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
+  PATH_VARS INCLUDE_INSTALL_DIR LIB_INSTALL_DIR
+)
+
+install(
+  FILES 
+  ${CMAKE_CURRENT_BINARY_DIR}/${PLUGIN_NAME}Config.cmake
+  DESTINATION 
+  ${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
+  $<INSTALL_INTERFACE:${INCLUDE_INSTALL_DIR}>
+)
 
-install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/cmake/${PLUGIN_NAME})
 #install headers
 INSTALL(FILES ${HDRS_CAPTURE}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/capture)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/capture)
 INSTALL(FILES ${HDRS_FACTOR}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/factor)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/factor)
 INSTALL(FILES ${HDRS_FEATURE}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/feature)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/feature)
 INSTALL(FILES ${HDRS_PROCESSOR}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/processor)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/processor)
 INSTALL(FILES ${HDRS_SENSOR}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/sensor)
 INSTALL(FILES ${HDRS_TREE_MANAGER}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/tree_manager)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/tree_manager)
 
-FILE(WRITE ${PROJECT_NAME}.found "")
-INSTALL(FILES ${PROJECT_NAME}.found
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME})
 INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/internal)
-
-INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/internal)
 
 INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
 
 export(PACKAGE ${PLUGIN_NAME})
 
-FIND_PACKAGE(Doxygen)
+FIND_PACKAGE(Doxygen MODULE)
 
 FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
 IF (IRI_DOC_DIR)
diff --git a/cmake_modules/wolfgnssConfig.cmake b/cmake_modules/wolfgnssConfig.cmake
deleted file mode 100644
index e55f091a2ce3b7150072fae7ab9fe018a5b49c19..0000000000000000000000000000000000000000
--- a/cmake_modules/wolfgnssConfig.cmake
+++ /dev/null
@@ -1,88 +0,0 @@
-#edit the following line to add the librarie's header files
-FIND_PATH(
-    wolfgnss_INCLUDE_DIRS
-    NAMES gnss.found
-    PATHS /usr/local/include/iri-algorithms/wolf/plugin_gnss)
-IF(wolfgnss_INCLUDE_DIRS)
-  MESSAGE("Found gnss include dirs: ${wolfgnss_INCLUDE_DIRS}")
-ELSE(wolfgnss_INCLUDE_DIRS)
-  MESSAGE("Couldn't find gnss include dirs")
-ENDIF(wolfgnss_INCLUDE_DIRS)
-
-FIND_LIBRARY(
-    wolfgnss_LIBRARIES
-    NAMES libwolfgnss.so
-    PATHS /usr/local/lib)
-IF(wolfgnss_LIBRARIES)
-  MESSAGE("Found gnss lib: ${wolfgnss_LIBRARIES}")
-ELSE(wolfgnss_LIBRARIES)
-  MESSAGE("Couldn't find wolf gnss lib")
-ENDIF(wolfgnss_LIBRARIES)
-
-IF (wolfgnss_INCLUDE_DIRS AND wolfgnss_LIBRARIES)
-   SET(wolfgnss_FOUND TRUE)
- ELSE(wolfgnss_INCLUDE_DIRS AND wolfgnss_LIBRARIES)
-   set(wolfgnss_FOUND FALSE)
-ENDIF (wolfgnss_INCLUDE_DIRS AND wolfgnss_LIBRARIES)
-
-IF (wolfgnss_FOUND)
-   IF (NOT wolfgnss_FIND_QUIETLY)
-      MESSAGE(STATUS "Found gnss: ${wolfgnss_LIBRARIES}")
-   ENDIF (NOT wolfgnss_FIND_QUIETLY)
-ELSE (wolfgnss_FOUND)
-   IF (wolfgnss_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find wolf gnss")
-   ENDIF (wolfgnss_FIND_REQUIRED)
-ENDIF (wolfgnss_FOUND)
-
-
-macro(wolf_report_not_found REASON_MSG)
-  set(wolfgnss_FOUND FALSE)
-  unset(wolfgnss_INCLUDE_DIRS)
-  unset(wolfgnss_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 (wolfgnss_FIND_QUIETLY)
-    message(STATUS "Failed to find wolf gnss- " ${REASON_MSG} ${ARGN})
-  elseif(wolfgnss_FIND_REQUIRED)
-    message(FATAL_ERROR "Failed to find wolf gnss - " ${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 gnss - " ${REASON_MSG} ${ARGN})
-  endif ()
-  return()
-endmacro(wolf_report_not_found)
-
-if(NOT wolfgnss_FOUND)
-  wolf_report_not_found("Something went wrong while setting up wolf gnss.")
-endif(NOT wolfgnss_FOUND)
-# Set the include directories for wolf (itself).
-set(wolfgnss_FOUND TRUE)
-
-# Now we gather all the required dependencies for Wolf Laser
-
-FIND_PACKAGE(gnss_utils REQUIRED)
-list(APPEND wolfgnss_INCLUDE_DIRS ${gnss_utils_INCLUDE_DIRS})
-list(APPEND wolfgnss_LIBRARIES ${gnss_utils_LIBRARY})
-if(NOT wolf_FOUND)
-  FIND_PACKAGE(wolfcore REQUIRED)
-
-  #We reverse in order to insert at the start
-  list(REVERSE wolfgnss_INCLUDE_DIRS)
-  list(APPEND wolfgnss_INCLUDE_DIRS ${wolfcore_INCLUDE_DIRS})
-  list(REVERSE wolfgnss_INCLUDE_DIRS)
-
-  list(REVERSE wolfgnss_LIBRARIES)
-  list(APPEND wolfgnss_LIBRARIES ${wolfcore_LIBRARIES})
-  list(REVERSE wolfgnss_LIBRARIES)
-endif()
-
-# provide both INCLUDE_DIR and INCLUDE_DIRS
-SET(wolfgnss_INCLUDE_DIR ${wolfgnss_INCLUDE_DIRS})
-# provide both LIBRARY and LIBRARIES 
-SET(wolfgnss_LIBRARY ${wolfgnss_LIBRARIES})
\ No newline at end of file
diff --git a/cmake_modules/wolfgnssConfig.cmake.in b/cmake_modules/wolfgnssConfig.cmake.in
new file mode 100644
index 0000000000000000000000000000000000000000..7ebd0fc6aebe72948aa6a456532abeb8f5951fe8
--- /dev/null
+++ b/cmake_modules/wolfgnssConfig.cmake.in
@@ -0,0 +1,17 @@
+set(@PLUGIN_NAME@_VERSION 0.0.1)
+
+
+@PACKAGE_INIT@
+
+set_and_check(@PLUGIN_NAME@_INCLUDE_DIR "@PACKAGE_INCLUDE_INSTALL_DIR@")
+set(@PLUGIN_NAME@_INCLUDE_DIRS @PLUGIN_NAME@_INCLUDE_DIR)
+set_and_check(@PLUGIN_NAME@_LIB_INSTALL_DIR "@PACKAGE_LIB_INSTALL_DIR@")
+set(@PLUGIN_NAME@_LIB_INSTALL_DIRS @PLUGIN_NAME@_LIB_INSTALL_DIR)
+
+# forwards the correct parameters given to FIND_DEPENDENCIES
+include(CMakeFindDependencyMacro)
+FIND_DEPENDENCY(gnss_utils REQUIRED)
+
+include("${CMAKE_CURRENT_LIST_DIR}/@PLUGIN_NAME@Targets.cmake")
+
+check_required_components(@PLUGIN_NAME@)
\ No newline at end of file
diff --git a/include/gnss/capture/capture_gnss.h b/include/gnss/capture/capture_gnss.h
index 1a8980534f1e38c6aa0b6479369fbe4db77c078f..ea0d900399a23860bcb53e3615af830834008d77 100644
--- a/include/gnss/capture/capture_gnss.h
+++ b/include/gnss/capture/capture_gnss.h
@@ -43,25 +43,43 @@ class CaptureGnss : public CaptureBase
       CaptureGnss(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, GnssUtils::SnapshotPtr _snapshot);
       ~CaptureGnss() override;
 
-      GnssUtils::SnapshotPtr getSnapshot() const;
-      GnssUtils::ObservationsPtr getObservations() const;
-      GnssUtils::NavigationPtr getNavigation() const;
-      GnssUtils::Satellites& getSatellites();
+      GnssUtils::SnapshotConstPtr getSnapshot() const;
+      GnssUtils::SnapshotPtr getSnapshot();
+      GnssUtils::ObservationsConstPtr getObservations() const;
+      GnssUtils::ObservationsPtr getObservations();
+      GnssUtils::NavigationConstPtr getNavigation() const;
+      GnssUtils::NavigationPtr getNavigation();
       const GnssUtils::Satellites& getSatellites() const;
+      GnssUtils::Satellites& getSatellites();
 
 };
 
-inline GnssUtils::SnapshotPtr CaptureGnss::getSnapshot() const
+inline GnssUtils::SnapshotConstPtr CaptureGnss::getSnapshot() const
 {
   return snapshot_;
 }
 
-inline GnssUtils::ObservationsPtr CaptureGnss::getObservations() const
+inline GnssUtils::SnapshotPtr CaptureGnss::getSnapshot()
+{
+  return snapshot_;
+}
+
+inline GnssUtils::ObservationsConstPtr CaptureGnss::getObservations() const
+{
+  return snapshot_->getObservations();
+}
+
+inline GnssUtils::ObservationsPtr CaptureGnss::getObservations()
 {
   return snapshot_->getObservations();
 }
 
-inline GnssUtils::NavigationPtr CaptureGnss::getNavigation() const
+inline GnssUtils::NavigationConstPtr CaptureGnss::getNavigation() const
+{
+  return snapshot_->getNavigation();
+}
+
+inline GnssUtils::NavigationPtr CaptureGnss::getNavigation()
 {
   return snapshot_->getNavigation();
 }
diff --git a/include/gnss/capture/capture_gnss_tdcp.h b/include/gnss/capture/capture_gnss_tdcp.h
index 73af63d7ccc75ce4ec45616638c60a54b43e722f..73dce5e723f89ecbf2ef88dc0fd13e7db692a944 100644
--- a/include/gnss/capture/capture_gnss_tdcp.h
+++ b/include/gnss/capture/capture_gnss_tdcp.h
@@ -56,7 +56,8 @@ class CaptureGnssTdcp : public CaptureBase
         const Eigen::Vector3d& getData() const;
         const Eigen::Matrix3d& getDataCovariance() const;
         void getDataAndCovariance(Eigen::Vector3d& data, Eigen::Matrix3d& data_cov) const;
-        FrameBasePtr getOriginFrame() const;
+        FrameBaseConstPtr getOriginFrame() const;
+        FrameBasePtr getOriginFrame();
         const TimeStamp& getGpsTimeStamp() const;
         void setGpsTimeStamp(const TimeStamp &_ts_GPST);
 };
@@ -77,12 +78,17 @@ inline void CaptureGnssTdcp::getDataAndCovariance(Eigen::Vector3d& data, Eigen::
     data_cov = data_covariance_;
 }
 
-inline FrameBasePtr CaptureGnssTdcp::getOriginFrame() const
+inline FrameBaseConstPtr CaptureGnssTdcp::getOriginFrame() const
 {
     return origin_frame_ptr_;
 }
 
-inline const wolf::TimeStamp& CaptureGnssTdcp::getGpsTimeStamp() const
+inline FrameBasePtr CaptureGnssTdcp::getOriginFrame()
+{
+    return origin_frame_ptr_;
+}
+
+inline const TimeStamp& CaptureGnssTdcp::getGpsTimeStamp() const
 {
     return ts_GPST_;
 }
diff --git a/include/gnss/factor/factor_gnss_single_diff_2d.h b/include/gnss/factor/factor_gnss_single_diff_2d.h
deleted file mode 100644
index f85495b058a4c7a33f8982f85f38d5b1e99c5b0b..0000000000000000000000000000000000000000
--- a/include/gnss/factor/factor_gnss_single_diff_2d.h
+++ /dev/null
@@ -1,126 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-
-#ifndef FACTOR_GNSS_SINGLE_DIFF_2d_H_
-#define FACTOR_GNSS_SINGLE_DIFF_2d_H_
-
-//Wolf includes
-#include "core/common/wolf.h"
-#include "core/factor/factor_autodiff.h"
-#include "core/frame/frame_base.h"
-#include "gnss/sensor/sensor_gnss.h"
-
-namespace wolf {
-    
-WOLF_PTR_TYPEDEFS(FactorGnssSingleDiff2d);
-
-class FactorGnssSingleDiff2d : public FactorAutodiff<FactorGnssSingleDiff2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>
-{
-    protected:
-        SensorGnssPtr sensor_gnss_ptr_;
-
-    public:
-
-        FactorGnssSingleDiff2d(FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
-            FactorAutodiff<FactorGnssSingleDiff2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>("GNSS SINGLE DIFFERENCES 2d",
-                                                                              TOP_GEOM,
-                                                                              _ftr_ptr,
-                                                                              _frame_other_ptr,
-                                                                              nullptr,
-                                                                              nullptr,
-                                                                              nullptr,
-                                                                              _processor_ptr,
-                                                                              _apply_loss_function,
-                                                                              _status,
-                                                                              _frame_other_ptr->getP(),
-                                                                              _frame_other_ptr->getO(),
-                                                                              _ftr_ptr->getFrame()->getP(),
-                                                                              _ftr_ptr->getFrame()->getO(),
-                                                                              _sensor_gnss_ptr->getP(),
-                                                                              _sensor_gnss_ptr->getEnuMapRoll(),
-                                                                              _sensor_gnss_ptr->getEnuMapPitch(),
-                                                                              _sensor_gnss_ptr->getEnuMapYaw()),
-            sensor_gnss_ptr_(_sensor_gnss_ptr)
-        {
-            WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 2d factor without initializing ENU");
-        }
-
-        virtual ~FactorGnssSingleDiff2d() = default;
-
-        template<typename T>
-        bool operator ()(const T* const _x1,
-                         const T* const _o1,
-                         const T* const _x2,
-                         const T* const _o2,
-                         const T* const _x_antena,
-                         const T* const _roll_ENU_MAP,
-                         const T* const _pitch_ENU_MAP,
-                         const T* const _yaw_ENU_MAP,
-                         T* _residuals) const;
-
-};
-
-template<typename T>
-inline bool FactorGnssSingleDiff2d::operator ()(const T* const _x1,
-                                                const T* const _o1,
-                                                const T* const _x2,
-                                                const T* const _o2,
-                                                const T* const _x_antena,
-                                                const T* const _roll_ENU_MAP,
-                                                const T* const _pitch_ENU_MAP,
-                                                const T* const _yaw_ENU_MAP,
-                                                T* _residuals) const
-{
-    Eigen::Map<const Eigen::Matrix<T,2,1> > t_MAP_BASE1(_x1);
-    Eigen::Matrix<T,2,2> R_MAP_BASE1 = Eigen::Rotation2D<T>(_o1[0]).matrix();
-    Eigen::Map<const Eigen::Matrix<T,2,1> > t_MAP_BASE2(_x2);
-    Eigen::Matrix<T,2,2> R_MAP_BASE2 = Eigen::Rotation2D<T>(_o2[0]).matrix();
-    Eigen::Map<const Eigen::Matrix<T,3,1> > t_BASE_ANTENA(_x_antena);
-    Eigen::Map<Eigen::Matrix<T,3,1> > residuals_ECEF(_residuals);
-
-    Eigen::Matrix<T,3,3> R_ENU_ECEF = sensor_gnss_ptr_->getREnuEcef().cast<T>();
-    Eigen::Matrix<T,2,3> R_MAP_ENU = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_MAP[0], _pitch_ENU_MAP[0], _yaw_ENU_MAP[0]).transpose().topRows(2);
-
-    // Transform ECEF 3d SingleDiff Feature to 2d SingleDiff in Map coordinates (removing z)
-    Eigen::Matrix<T,2,1> measured_diff_2d_MAP = R_MAP_ENU * (R_ENU_ECEF * getMeasurement().cast<T>());
-
-    // Substraction of expected antena positions in Map coordinates
-    Eigen::Matrix<T,2,1> expected_diff_2d_MAP = (R_MAP_BASE2 * t_BASE_ANTENA.head(2) + t_MAP_BASE2) - (R_MAP_BASE1 * t_BASE_ANTENA.head(2) + t_MAP_BASE1);
-
-    // Compute residual rotating information matrix to 2d Map coordinates
-    // Covariance & information are rotated by R*S*R' so for the squred root upper (or right) R*L*U*R'->U*R'
-    // In this case R = R_2dMAP_ENU * R_ENU_ECEF, then R' = R_ENU_ECEF' * R_2dMAP_ENU'
-    residuals_ECEF = (getMeasurementSquareRootInformationUpper().cast<T>() * R_ENU_ECEF.transpose() * R_MAP_ENU.transpose()) * (expected_diff_2d_MAP - measured_diff_2d_MAP);
-
-    //std::cout << "frame1: " << _x1[0] << " " << _x1[1] << " " << _o1[0] << std::endl;
-    //std::cout << "frame2: " << _x2[0] << " " << _x2[1] << " " << _o2[0] << std::endl;
-    //std::cout << "antena: " << _x_antena[0] << " " << _x_antena[1] << " " << _x_antena[2] << std::endl;
-    //std::cout << "RPY: " << _roll[0] << " " << _pitch[1] << " " << _yaw[2] << std::endl;
-    //std::cout << "measured_diff_2d: " << measured_diff_2d[0] << " " << measured_diff_2d[1] << std::endl;
-    //std::cout << "expected_diff_2d: " << expected_diff_2d[0] << " " << expected_diff_2d[1] << std::endl;
-
-    return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/gnss/processor/processor_gnss_tdcp.h b/include/gnss/processor/processor_gnss_tdcp.h
index 0f1bbf1d0aed54d393109a3d63f7769a3a3aea5d..caca1debbd9e2add16bf14dfd522e97ba96a2546 100644
--- a/include/gnss/processor/processor_gnss_tdcp.h
+++ b/include/gnss/processor/processor_gnss_tdcp.h
@@ -96,6 +96,7 @@ class ProcessorGnssTdcp : public ProcessorBase
 
         WOLF_PROCESSOR_CREATE(ProcessorGnssTdcp, ParamsProcessorGnssTdcp);
 
+        FrameBaseConstPtr getLastKF() const;
         FrameBasePtr getLastKF();
 
     protected:
@@ -144,7 +145,12 @@ class ProcessorGnssTdcp : public ProcessorBase
 
 };
 
-inline wolf::FrameBasePtr ProcessorGnssTdcp::getLastKF()
+inline FrameBaseConstPtr ProcessorGnssTdcp::getLastKF() const
+{
+    return last_KF_;
+}
+
+inline FrameBasePtr ProcessorGnssTdcp::getLastKF()
 {
     return last_KF_;
 }
diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h
index 03fc7a2d446f4b568939e1686ccb021f234f1129..fece2683601328edd6d6f86b0efc5ba64ce36dc8 100644
--- a/include/gnss/sensor/sensor_gnss.h
+++ b/include/gnss/sensor/sensor_gnss.h
@@ -110,10 +110,14 @@ class SensorGnss : public SensorBase
         ~SensorGnss() override;
 
         // Gets
-        StateBlockPtr getEnuMapTranslation() const;
-        StateBlockPtr getEnuMapRoll() const;
-        StateBlockPtr getEnuMapPitch() const;
-        StateBlockPtr getEnuMapYaw() const;
+        StateBlockConstPtr getEnuMapTranslation() const;
+        StateBlockPtr getEnuMapTranslation();
+        StateBlockConstPtr getEnuMapRoll() const;
+        StateBlockPtr getEnuMapRoll();
+        StateBlockConstPtr getEnuMapPitch() const;
+        StateBlockPtr getEnuMapPitch();
+        StateBlockConstPtr getEnuMapYaw() const;
+        StateBlockPtr getEnuMapYaw();
         const Eigen::Matrix3d& getREnuEcef() const;
         const Eigen::Vector3d& gettEnuEcef() const;
         Eigen::Matrix3d getREnuMap() const;
@@ -178,22 +182,42 @@ inline bool SensorGnss::isEnuModeAuto() const
     return params_->ENU_mode == "auto";
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapTranslation() const
+inline StateBlockConstPtr SensorGnss::getEnuMapTranslation() const
 {
     return getStateBlock('t');
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapRoll() const
+inline StateBlockPtr SensorGnss::getEnuMapTranslation()
+{
+    return getStateBlock('t');
+}
+
+inline StateBlockConstPtr SensorGnss::getEnuMapRoll() const
+{
+    return getStateBlock('r');
+}
+
+inline StateBlockPtr SensorGnss::getEnuMapRoll()
 {
     return getStateBlock('r');
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapPitch() const
+inline StateBlockConstPtr SensorGnss::getEnuMapPitch() const
 {
     return getStateBlock('p');
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapYaw() const
+inline StateBlockPtr SensorGnss::getEnuMapPitch()
+{
+    return getStateBlock('p');
+}
+
+inline StateBlockConstPtr SensorGnss::getEnuMapYaw() const
+{
+    return getStateBlock('y');
+}
+
+inline StateBlockPtr SensorGnss::getEnuMapYaw()
 {
     return getStateBlock('y');
 }
diff --git a/internal/config.h.in b/internal/config.h.in
index 7014ca320e5c5fea1f59a6fe5d9d1ea0987bf95e..35f3468331656ba09a63ad7f26e5c0fb78c8d5bc 100644
--- a/internal/config.h.in
+++ b/internal/config.h.in
@@ -24,13 +24,13 @@
 //            which will be added to the include path for compilation,
 //            and installed with the public wolf headers.
 
-#ifndef WOLF_INTERNAL_${UPPER_NAME}_CONFIG_H_
-#define WOLF_INTERNAL_${UPPER_NAME}_CONFIG_H_
+#ifndef WOLF_INTERNAL_${PROJECT_NAME_UPPER}_CONFIG_H_
+#define WOLF_INTERNAL_${PROJECT_NAME_UPPER}_CONFIG_H_
 
 #cmakedefine _WOLF_DEBUG
 
 #cmakedefine _WOLF_TRACE
 
-#define _WOLF_${UPPER_NAME}_ROOT_DIR "${_WOLF_ROOT_DIR}"
+#define _WOLF_${PROJECT_NAME_UPPER}_ROOT_DIR "${_WOLF_ROOT_DIR}"
 
 #endif /* WOLF_INTERNAL_CONFIG_H_ */
diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp
deleted file mode 100644
index 99c73f2fc8b8fb824739dde680a1264dbe8e842b..0000000000000000000000000000000000000000
--- a/src/processor/processor_gnss_single_diff.cpp
+++ /dev/null
@@ -1,179 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#include "gnss/factor/factor_gnss_single_diff_2d.h"
-#include "gnss/feature/feature_gnss_single_diff.h"
-#include "gnss/processor/processor_gnss_single_diff.h"
-#include "gnss/capture/capture_gnss_single_diff.h"
-
-namespace wolf
-{
-
-ProcessorGnssSingleDiff::ProcessorGnssSingleDiff(ParamsProcessorGnssSingleDiffPtr _params_gnss) :
-        ProcessorBase("ProcessorGnssSingleDiff", 0, _params_gnss),
-        params_gnss_(_params_gnss)
-{
-    //
-}
-
-ProcessorGnssSingleDiff::~ProcessorGnssSingleDiff()
-{
-    //
-}
-
-void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture)
-{
-    // TODO: keep captures in a buffer and deal with KFpacks
-
-    //WOLF_DEBUG("ProcessorGnssSingleDiff::process()");
-    incoming_capture_ = std::static_pointer_cast<CaptureGnssSingleDiff>(_capture);
-
-    // discard capture with null or non-key origin frame
-    if (incoming_capture_->getOriginFrame() == nullptr || !incoming_capture_->getOriginFrame()->isKey())
-    {
-        WOLF_WARN("process single difference with null frame origin, skipping...");
-        return;
-    }
-
-    if (last_KF_ == nullptr)
-        last_KF_ = incoming_capture_->getOriginFrame();
-
-    // NEW KF? ------------------------------------------------
-    FrameBasePtr new_frame = nullptr;
-
-    // ALREADY CREATED KF
-    PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( incoming_capture_, params_->time_tolerance);
-    if (KF_pack && KF_pack->key_frame != incoming_capture_->getOriginFrame())
-    {
-        new_frame = KF_pack->key_frame;
-        WOLF_DEBUG( "PR ",getName()," - capture ", incoming_capture_->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp());
-    }
-    // MAKE KF
-    else if (voteForKeyFrame() && permittedKeyFrame())
-    {
-        new_frame = getProblem()->emplaceKeyFrame( incoming_capture_->getTimeStamp());
-        getProblem()->keyFrameCallback(new_frame, shared_from_this(), params_->time_tolerance);
-        WOLF_DEBUG( "PR ",getName()," - capture ", incoming_capture_->id(), " appended to new KF " , new_frame->id() , " TS: ", new_frame->getTimeStamp());
-    }
-
-    // ESTABLISH FACTOR ------------------------------------------------
-    if (new_frame)
-    {
-        // LINK CAPTURE
-        _capture->link(new_frame); // Add incoming Capture to the new Frame
-
-        // EMPLACE FEATURE
-        auto ftr = FeatureBase::emplace<FeatureGnssSingleDiff>(incoming_capture_, incoming_capture_->getData(),incoming_capture_->getDataCovariance());
-
-        // ADD FACTOR
-        emplaceFactor(ftr);
-
-        // store last KF
-        last_KF_ = new_frame;
-    }
-
-    // INITIALIZE ENU_MAP IF 4 NECESSARY CONDITIONS ------------------------------------------------
-    //      1 - ENU-ECEF defined
-    //      2 - not initialized
-    //      3 - current capture in key-frame with factor
-    //      4 - frames constained by the factor separated enough ( > enu_map_init_dist_min)
-    if ( sensor_gnss_->isEnuDefined() &&
-        !sensor_gnss_->isEnuMapInitialized() &&
-         new_frame != nullptr &&
-         incoming_capture_->getFrame() != nullptr && incoming_capture_->getFrame()->isKey() &&
-         incoming_capture_->getData().norm() > params_gnss_->enu_map_init_dist_min)
-    {
-        WOLF_DEBUG("initializing enu map");
-        sensor_gnss_->initializeEnuMapYaw(incoming_capture_->getOriginFrame()->getState(),
-                                          incoming_capture_->getFrame()->getState(),
-                                          incoming_capture_->getData());
-    }
-}
-
-FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr _ftr)
-{
-    //WOLF_DEBUG("creating the factor...");
-    // 2d
-    if (getProblem()->getDim() == 2)
-        return FactorBase::emplace<FactorGnssSingleDiff2d>(_ftr, _ftr, incoming_capture_->getOriginFrame(), sensor_gnss_, shared_from_this(), params_->apply_loss_function);
-    // 3d TODO
-    else
-        std::runtime_error("Single Differences in 3d not implemented yet.");
-
-    return nullptr;
-}
-
-bool ProcessorGnssSingleDiff::voteForKeyFrame() const
-{
-    if (last_KF_==nullptr)
-        return true;
-
-    std::cout << "params_gnss_->time_th = " << params_gnss_->time_th << std::endl;
-    std::cout << "(last_KF_->getTimeStamp() - incoming_capture_->getTimeStamp()) = " << (last_KF_->getTimeStamp() - incoming_capture_->getTimeStamp()) << std::endl;
-
-    // Depending on time since the last KF with gnssfix capture
-    if ((incoming_capture_->getTimeStamp() - last_KF_->getTimeStamp()) > params_gnss_->time_th)
-        return true;
-
-    // Distance criterion
-    std::cout << "params_gnss_->dist_traveled = " << params_gnss_->dist_traveled << std::endl;
-    Eigen::Vector2d v_current_origin = (sensor_gnss_->getREnuMap().transpose() * sensor_gnss_->getREnuEcef() * incoming_capture_->getData()).head<2>();
-    std::cout << "v_current_origin: " << v_current_origin.transpose() << std::endl;
-    Eigen::Vector2d v_origin_last_KF = last_KF_->getP()->getState() - incoming_capture_->getOriginFrame()->getP()->getState();
-    std::cout << "v_origin_last_KF: " << v_origin_last_KF.transpose() << std::endl;
-    std::cout << "v_current_origin + v_origin_last_KF: " << (v_current_origin + v_origin_last_KF).transpose() << std::endl;
-    if ((v_current_origin + v_origin_last_KF).norm() > params_gnss_->dist_traveled)
-        return true;
-
-    // TODO: more alternatives?
-
-    // otherwise
-    return false;
-}
-
-bool ProcessorGnssSingleDiff::storeKeyFrame(FrameBasePtr _frame_ptr)
-{
-    return true;
-}
-bool ProcessorGnssSingleDiff::storeCapture(CaptureBasePtr _cap_ptr)
-{
-    return false;
-}
-void ProcessorGnssSingleDiff::configure(SensorBasePtr _sensor)
-{
-    sensor_gnss_ = std::static_pointer_cast<SensorGnss>(_sensor);
-};
-
-ProcessorBasePtr ProcessorGnssSingleDiff::create(const std::string& _unique_name, const ParamsProcessorBasePtr _params)
-{
-    ProcessorGnssSingleDiffPtr prc = std::make_shared<ProcessorGnssSingleDiff>(std::static_pointer_cast<ParamsProcessorGnssSingleDiff>(_params));
-    prc->setName(_unique_name);
-    return prc;
-}
-
-} // namespace wolf
-
-
-// Register in the FactorySensor
-#include "core/processor/factory_processor.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR(ProcessorGnssSingleDiff)
-} // namespace wolf
diff --git a/src/processor/processor_gnss_tdcp.cpp b/src/processor/processor_gnss_tdcp.cpp
index c087f70747614b859de670898ce9cd9dd655ca6f..1ed2b6855410743a787dc6f6edadd9ae0b6ab6ab 100644
--- a/src/processor/processor_gnss_tdcp.cpp
+++ b/src/processor/processor_gnss_tdcp.cpp
@@ -144,11 +144,15 @@ void ProcessorGnssTdcp::processKeyFrame(FrameBasePtr _keyframe)
     }
 
     // Iterate over all KF of the trajectory with GNSS captures
-    for (auto KF_it = getProblem()->getTrajectory()->rbegin();
-         KF_it != getProblem()->getTrajectory()->rend();
-         KF_it++)
+    auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+    for (auto frame_rev_iter = frame_map.rbegin();
+        frame_rev_iter != frame_map.rend();
+        ++frame_rev_iter)
     {
-        auto KF_ref = *KF_it;
+        auto KF_ref = frame_rev_iter->second;
+        if (not KF_ref)
+            break;
+
         if (_keyframe->getTimeStamp() < KF_ref->getTimeStamp())
             continue;
 
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index ace52948ce6e00adc5961081c00ee02fe8760781..802a7ca5f1c385da8335b9512121a0ebce5f154b 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -410,21 +410,21 @@ void ProcessorTrackerGnss::establishFactors()
             WOLF_DEBUG("TDCP BATCH frame ", last_ptr_->getFrame()->id());
             FactorBasePtr last_fac_ptr = nullptr;
 
-            for (auto KF_rit = getProblem()->getTrajectory()->rbegin();
-                 KF_rit != getProblem()->getTrajectory()->rend();
-                 KF_rit++)
+            auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+            for (auto frame_rev_iter = frame_map.rbegin();
+                frame_rev_iter != frame_map.rend();
+                ++frame_rev_iter)
             {
-                FrameBasePtr KF = (*KF_rit);
-
-                WOLF_DEBUG("TDCP BATCH ref frame ", KF->id());
+                auto ref_KF = frame_rev_iter->second;
+                WOLF_DEBUG("TDCP BATCH ref frame ", ref_KF->id());
 
                 // discard non-key frames, last-last pair and frames without CaptureGnss
-                if (KF == last_ptr_->getFrame() or
-                    KF->getCaptureOf(getSensor(),"CaptureGnss") == nullptr)
+                if (ref_KF == last_ptr_->getFrame() or
+                    ref_KF->getCaptureOf(getSensor(),"CaptureGnss") == nullptr)
                     continue;
 
                 // static cast
-                auto ref_cap_gnss = std::static_pointer_cast<CaptureGnss>((*KF_rit)->getCaptureOf(getSensor(),"CaptureGnss"));
+                auto ref_cap_gnss = std::static_pointer_cast<CaptureGnss>(ref_KF->getCaptureOf(getSensor(),"CaptureGnss"));
                 auto last_cap_gnss = std::static_pointer_cast<CaptureGnss>(last_ptr_);
 
                 // dt
@@ -469,7 +469,7 @@ void ProcessorTrackerGnss::establishFactors()
                 //std::cout << std::endl;
 
                 // reference ECEF position
-                Eigen::Vector3d x_r = sensor_gnss_->computeFrameAntennaPosEcef(KF);
+                Eigen::Vector3d x_r = sensor_gnss_->computeFrameAntennaPosEcef(ref_KF);
 
                 // compute TDCP batch
                 auto tdcp_output = GnssUtils::Tdcp(ref_cap_gnss->getSnapshot(),
@@ -527,7 +527,8 @@ void ProcessorTrackerGnss::establishFactors()
             {
                 // current feature
                 auto ftr_k = std::dynamic_pointer_cast<FeatureGnssSatellite>(ftr_k_pair.second);
-                assert(ftr_k != nullptr);
+                if(not ftr_k)
+                    continue;
 
                 // check valid measurement
                 assert(std::abs(ftr_k->getObservation().L[0]) > 1e-12);
@@ -543,7 +544,8 @@ void ProcessorTrackerGnss::establishFactors()
                 {
                     // cast reference feature
                     auto ftr_r = std::dynamic_pointer_cast<FeatureGnssSatellite>(ts_ftr_r_it->second);
-                    assert(ftr_r != nullptr);
+                    if(not ftr_r)
+                        continue;
 
                     // dt
                     double dt = ftr_k->getCapture()->getTimeStamp() - ts_ftr_r_it->first;
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index fc829135d4db9a3019a429f1a5195ed38fa160d4..eced537f0d3e20ff77e2764e857919f6b04a629f 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -1,26 +1,19 @@
 # Retrieve googletest from github & compile
 add_subdirectory(gtest)
 
-# Include gtest directory.
-include_directories(${GTEST_INCLUDE_DIRS})
-
 ############# USE THIS TEST AS AN EXAMPLE #################
 #                                                         #
 # Create a specific test executable for gtest_example     #
 # wolf_add_gtest(gtest_example gtest_example.cpp)         #
-# target_link_libraries(gtest_example ${PLUGIN_NAME})     #
 #                                                         #
 ###########################################################
 
 # FactorGnssFix2d test
 wolf_add_gtest(gtest_factor_gnss_fix_2d gtest_factor_gnss_fix_2d.cpp)
-target_link_libraries(gtest_factor_gnss_fix_2d ${PLUGIN_NAME})
 
 # FactorGnssPseudoRange test
 wolf_add_gtest(gtest_factor_gnss_pseudo_range gtest_factor_gnss_pseudo_range.cpp)
-target_link_libraries(gtest_factor_gnss_pseudo_range ${PLUGIN_NAME})
 
 # FactorGnssTdcp test
 wolf_add_gtest(gtest_factor_gnss_tdcp gtest_factor_gnss_tdcp.cpp)
-target_link_libraries(gtest_factor_gnss_tdcp ${PLUGIN_NAME})
 
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
index 559756b02472653c655c44152c452ba8767ef6f2..6209d341bcd32308bd522ab1990eb0cc82692012 100644
--- a/test/gtest/CMakeLists.txt
+++ b/test/gtest/CMakeLists.txt
@@ -1,65 +1,73 @@
-cmake_minimum_required(VERSION 2.8.8)
-project(gtest_builder C CXX)
+if(${CMAKE_VERSION} VERSION_LESS "3.11.0") 
+  message("CMake version less than 3.11.0")
 
-# We need thread support
-#find_package(Threads REQUIRED)
+  # Enable ExternalProject CMake module
+  include(ExternalProject)
 
-# Enable ExternalProject CMake module
-include(ExternalProject)
+  set(GTEST_FORCE_SHARED_CRT ON)
+  set(GTEST_DISABLE_PTHREADS ON) # without this in ubuntu 18.04 we get linking errors
 
-set(GTEST_FORCE_SHARED_CRT ON)
-set(GTEST_DISABLE_PTHREADS OFF)
+  # Download GoogleTest
+  ExternalProject_Add(googletest
+      GIT_REPOSITORY https://github.com/google/googletest.git
+      GIT_TAG        v1.8.x
+      # TIMEOUT 1 # We'll try this
+      CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
+      -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
+      -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
+      -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
+      -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
+      -DBUILD_GTEST=ON
+      PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
+      # Disable install step
+      INSTALL_COMMAND ""
+      UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
+  )
 
-# For some reason I need to disable PTHREADS
-# with g++ (Ubuntu 4.9.3-8ubuntu2~14.04) 4.9.3
-# This is a known issue for MinGW :
-# https://github.com/google/shaderc/pull/174
-#if(MINGW)
-    set(GTEST_DISABLE_PTHREADS ON)
-#endif()
+  # Get GTest source and binary directories from CMake project
 
-# Download GoogleTest
-ExternalProject_Add(googletest
-    GIT_REPOSITORY https://github.com/google/googletest.git
-    GIT_TAG        v1.8.x
-    # TIMEOUT 1 # We'll try this
-    CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
-    -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
-    -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
-    -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
-    -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
-    -DBUILD_GTEST=ON
-    PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
-    # Disable install step
-    INSTALL_COMMAND ""
-    UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
-)
+  # Specify include dir
+  ExternalProject_Get_Property(googletest source_dir)
+  set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
 
-# Get GTest source and binary directories from CMake project
+  # Specify MainTest's link libraries
+  ExternalProject_Get_Property(googletest binary_dir)
+  set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
 
-# Specify include dir
-ExternalProject_Get_Property(googletest source_dir)
-set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
+  # Create a libgtest target to be used as a dependency by test programs
+  add_library(libgtest IMPORTED STATIC GLOBAL)
+  add_dependencies(libgtest googletest)
 
-# Specify MainTest's link libraries
-ExternalProject_Get_Property(googletest binary_dir)
-set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
+  # Set libgtest properties
+  set_target_properties(libgtest PROPERTIES
+      "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
+      "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
+  )
 
-# Create a libgtest target to be used as a dependency by test programs
-add_library(libgtest IMPORTED STATIC GLOBAL)
-add_dependencies(libgtest googletest)
+else()
 
-# Set libgtest properties
-set_target_properties(libgtest PROPERTIES
-    "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
-    "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
-)
+  message("CMake version equal or greater than 3.11.0")
 
+  include(FetchContent)
+
+  FetchContent_Declare(
+    googletest
+    GIT_REPOSITORY https://github.com/google/googletest.git 
+    GIT_TAG main)
+
+  SET(INSTALL_GTEST OFF) # Disable installation of googletest
+  FetchContent_MakeAvailable(googletest)
+    
+endif()
+  
 function(wolf_add_gtest target)
   add_executable(${target} ${ARGN})
-  add_dependencies(${target} libgtest)
-  target_link_libraries(${target} libgtest)
-
-  #WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/bin
+  if(${CMAKE_VERSION} VERSION_LESS "3.11.0") 
+    add_dependencies(${target} libgtest)
+    target_link_libraries(${target} libgtest ${PLUGIN_NAME})
+    target_include_directories(${target} PUBLIC ${GTEST_INCLUDE_DIRS})
+  else()
+    target_link_libraries(${target} gtest_main ${PLUGIN_NAME})
+  endif()
   add_test(NAME ${target} COMMAND ${target})
 endfunction()
diff --git a/test/gtest_factor_gnss_pseudo_range.cpp b/test/gtest_factor_gnss_pseudo_range.cpp
index f6691f178df5d7697e1d3f75d0e1dfb32af21598..91fbaa28c0a8768f7088e902f71c50ddfec242de 100644
--- a/test/gtest_factor_gnss_pseudo_range.cpp
+++ b/test/gtest_factor_gnss_pseudo_range.cpp
@@ -179,7 +179,7 @@ void fixAllStates()
 ////////////////////////////////////////////////////////
 TEST(FactorGnssPreusoRangeTest, observe_clock_drift)
 {
-    for (auto i = 0; i < 100; i++)
+    for (auto i = 0; i < 40; i++)
     {
         // setup random problem
         randomGroundtruth();
@@ -207,7 +207,7 @@ TEST(FactorGnssPreusoRangeTest, observe_clock_drift)
 
 TEST(FactorGnssPreusoRangeTest, observe_frame_p)
 {
-    for (auto i = 0; i < 100; i++)
+    for (auto i = 0; i < 40; i++)
     {
         // setup random problem
         randomGroundtruth();
@@ -233,7 +233,7 @@ TEST(FactorGnssPreusoRangeTest, observe_frame_p)
 
 TEST(FactorGnssPreusoRangeTest, observe_frame_p_clock)
 {
-    for (auto i = 0; i < 100; i++)
+    for (auto i = 0; i < 40; i++)
     {
         // setup random problem
         randomGroundtruth();
@@ -261,7 +261,7 @@ TEST(FactorGnssPreusoRangeTest, observe_frame_p_clock)
 
 TEST(FactorGnssPreusoRangeTest, observe_enumap_p)
 {
-    for (auto i = 0; i < 100; i++)
+    for (auto i = 0; i < 40; i++)
     {
         // setup random problem
         randomGroundtruth();
@@ -287,7 +287,7 @@ TEST(FactorGnssPreusoRangeTest, observe_enumap_p)
 
 TEST(FactorGnssPreusoRangeTest, observe_enumap_o)
 {
-    for (auto i = 0; i < 100; i++)
+    for (auto i = 0; i < 40; i++)
     {
         // setup random problem
         randomGroundtruth();