diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index c841e751f22dc2c48852fea378b5df0a2f3c80e3..ff1e2eb36d460125006290be0be82e37296c54e9 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -5,9 +5,10 @@ stages:
 ############ YAML ANCHORS ############
 .print_variables_template: &print_variables_definition
   # Print variables
+  - echo $WOLF_CORE_BRANCH
   - echo $CI_COMMIT_BRANCH
-  - echo $WOLF_IMU_BRANCH
   - echo $WOLF_BODYDYNAMICS_BRANCH
+  - echo $WOLF_IMU_BRANCH
 
 .preliminaries_template: &preliminaries_definition
   ## Install ssh-agent if not already installed, it is required by Docker.
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2b7248061485afe23b1315e3fc46b4d4e7c170d3..9ca4dd5c2fccc846c026b27a12f511560f160c58 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,10 +1,6 @@
 # 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)
-  cmake_policy(SET CMP0003 NEW)
-endif(COMMAND cmake_policy)
 # MAC OSX RPATH
 SET(CMAKE_MACOSX_RPATH 1)
 
@@ -16,7 +12,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 +64,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 +72,14 @@ ENDIF()
 option(_WOLF_TRACE "Enable wolf tracing macro" ON)
 
 # ============ DEPENDENCIES  ============
-FIND_PACKAGE(wolfcore REQUIRED)
+FIND_PACKAGE(wolfcore REQUIRED CONFIG)
 FIND_PACKAGE(wolfimu REQUIRED)
+FIND_PACKAGE(Eigen3 3.3 REQUIRED CONFIG)
 
 # ============ 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)
@@ -103,7 +98,6 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_D
 include_directories("${PROJECT_BINARY_DIR}/conf")
 
 # ============ INCLUDES ============ 
-INCLUDE_DIRECTORIES(${wolfimu_INCLUDE_DIRS})
 INCLUDE_DIRECTORIES(BEFORE "include")
 
 # ============ HEADERS ============ 
@@ -185,7 +179,9 @@ endif()
 
 #Link the created libraries
 #===============EXAMPLE=========================
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfimu_LIBRARIES})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC wolfcore)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC wolfimu)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC Eigen3::Eigen)
 
 # Build demos
 IF(BUILD_DEMOS)
@@ -199,14 +195,38 @@ IF(BUILD_TESTS)
   add_subdirectory(test)
 ENDIF(BUILD_TESTS)
 
+
 #install library
 #=============================================================
 INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
   RUNTIME DESTINATION bin
-  LIBRARY DESTINATION lib
-  ARCHIVE DESTINATION lib)
+  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)
@@ -221,17 +241,12 @@ INSTALL(FILES ${HDRS_PROCESSOR}
 INSTALL(FILES ${HDRS_SENSOR}
   DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
 
-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}")
-
 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/bodydynamics.found b/bodydynamics.found
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/cmake_modules/wolfbodydynamicsConfig.cmake b/cmake_modules/wolfbodydynamicsConfig.cmake
deleted file mode 100644
index cfb538c1d7d49ef39ea0f6994488f9bc59d27724..0000000000000000000000000000000000000000
--- a/cmake_modules/wolfbodydynamicsConfig.cmake
+++ /dev/null
@@ -1,91 +0,0 @@
-#edit the following line to add the librarie's header files
-FIND_PATH(
-    wolfbodydynamics_INCLUDE_DIRS
-    NAMES bodydynamics.found
-    PATHS /usr/local/include/iri-algorithms/wolf/plugin_bodydynamics)
-IF(wolfbodydynamics_INCLUDE_DIRS)
-  MESSAGE("Found wolf bodydynamics include dirs: ${wolfbodydynamics_INCLUDE_DIRS}")
-ELSE(wolfbodydynamics_INCLUDE_DIRS)
-  MESSAGE("Couldn't find wolf bodydynamics include dirs")
-ENDIF(wolfbodydynamics_INCLUDE_DIRS)
-
-FIND_LIBRARY(
-    wolfbodydynamics_LIBRARIES
-    NAMES libwolfbodydynamics.so
-    PATHS /usr/local/lib)
-IF(wolfbodydynamics_LIBRARIES)
-  MESSAGE("Found wolf bodydynamics lib: ${wolfbodydynamics_LIBRARIES}")
-ELSE(wolfbodydynamics_LIBRARIES)
-  MESSAGE("Couldn't find wolf bodydynamics lib")
-ENDIF(wolfbodydynamics_LIBRARIES)
-
-IF (wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
-   SET(wolfbodydynamics_FOUND TRUE)
- ELSE(wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
-   set(wolfbodydynamics_FOUND FALSE)
-ENDIF (wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
-
-IF (wolfbodydynamics_FOUND)
-   IF (NOT wolfbodydynamics_FIND_QUIETLY)
-      MESSAGE(STATUS "Found wolf bodydynamics: ${wolfbodydynamics_LIBRARIES}")
-   ENDIF (NOT wolfbodydynamics_FIND_QUIETLY)
-ELSE (wolfbodydynamics_FOUND)
-   IF (wolfbodydynamics_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find wolf bodydynamics")
-   ENDIF (wolfbodydynamics_FIND_REQUIRED)
-ENDIF (wolfbodydynamics_FOUND)
-
-
-macro(wolf_report_not_found REASON_MSG)
-  set(wolfbodydynamics_FOUND FALSE)
-  unset(wolfbodydynamics_INCLUDE_DIRS)
-  unset(wolfbodydynamics_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 (wolfbodydynamics_FIND_QUIETLY)
-    message(STATUS "Failed to find wolf bodydynamics- " ${REASON_MSG} ${ARGN})
-  elseif(wolfbodydynamics_FIND_REQUIRED)
-    message(FATAL_ERROR "Failed to find wolf bodydynamics - " ${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 bodydynamics - " ${REASON_MSG} ${ARGN})
-  endif ()
-  return()
-endmacro(wolf_report_not_found)
-
-if(NOT wolfbodydynamics_FOUND)
-  wolf_report_not_found("Something went wrong while setting up wolf bodydynamics.")
-endif(NOT wolfbodydynamics_FOUND)
-# Set the include directories for wolf (itself).
-set(wolfbodydynamics_FOUND TRUE)
-
-# Now we gather all the required dependencies for Wolf Template
-
-#EXAMPLE Suppose that Wolf Template depends on laser_scan_utils
-# FIND_PACKAGE(laser_scan_utils REQUIRED)
-# list(APPEND wolflaser_INCLUDE_DIRS ${laser_scan_utils_INCLUDE_DIRS})
-# list(APPEND wolflaser_LIBRARIES ${laser_scan_utils_LIBRARY})
-
-# Making sure that wolf is always looked for
-if(NOT wolf_FOUND)
-  FIND_PACKAGE(wolfcore REQUIRED)
-
-  #We reverse in order to insert at the start
-  list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
-  list(APPEND wolfbodydynamics_INCLUDE_DIRS ${wolfcore_INCLUDE_DIRS})
-  list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
-
-  list(REVERSE wolfbodydynamics_LIBRARIES)
-  list(APPEND wolfbodydynamics_LIBRARIES ${wolfcore_LIBRARIES})
-  list(REVERSE wolfbodydynamics_LIBRARIES)
-endif()
-
-# provide both INCLUDE_DIR and INCLUDE_DIRS
-SET(wolfbodydynamics_INCLUDE_DIR ${wolfbodydynamics_INCLUDE_DIRS})
-# provide both LIBRARY and LIBRARIES 
-SET(wolfbodydynamics_LIBRARY ${wolfbodydynamics_LIBRARIES})
\ No newline at end of file
diff --git a/cmake_modules/wolfbodydynamicsConfig.cmake.in b/cmake_modules/wolfbodydynamicsConfig.cmake.in
new file mode 100644
index 0000000000000000000000000000000000000000..0722eb6dafef443a92638b2e888219c9fe0bd017
--- /dev/null
+++ b/cmake_modules/wolfbodydynamicsConfig.cmake.in
@@ -0,0 +1,13 @@
+set(@PLUGIN_NAME@_VERSION 0.0.1)
+
+
+@PACKAGE_INIT@
+
+include(CMakeFindDependencyMacro)
+FIND_DEPENDENCY(wolfcore REQUIRED)
+FIND_DEPENDENCY(wolfimu REQUIRED)
+FIND_DEPENDENCY(Eigen3 3.3 REQUIRED)
+
+include("${CMAKE_CURRENT_LIST_DIR}/@PLUGIN_NAME@Targets.cmake")
+
+check_required_components(@PLUGIN_NAME@)
\ No newline at end of file
diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
index ce0f3ecb7bf989be57688faae510ac3320114188..78d2b265912a542724c6ccaa3609638dc49259e2 100644
--- a/demos/CMakeLists.txt
+++ b/demos/CMakeLists.txt
@@ -8,48 +8,43 @@ if (pinocchio_FOUND)
 	include_directories(
 	    SYSTEM ${PINOCCHIO_INCLUDE_DIRS}
 	)
+
+	add_library(mcapi_utils mcapi_utils.cpp)
+	target_link_libraries(mcapi_utils Eigen3::Eigen)
+
 	add_executable(solo_real_povcdl_estimation solo_real_povcdl_estimation.cpp)
 	target_link_libraries(solo_real_povcdl_estimation 
-	    mcapi_utils
-	    ${wolfcore_LIBRARIES}
-	    ${wolfimu_LIBRARIES}
+	    wolfcore
+	    wolfimu
 	    ${PLUGIN_NAME}
 	    ${pinocchio_LIBRARIES}    
 	    /usr/local/lib/libcnpy.so
 	    z
+		mcapi_utils
 	)
-	
-	add_executable(solo_real_pov_estimation solo_real_pov_estimation.cpp)
-	target_link_libraries(solo_real_pov_estimation 
-	    mcapi_utils
-	    ${wolfcore_LIBRARIES}
-	    ${wolfimu_LIBRARIES}
+
+	add_executable(solo_imu_mocap solo_imu_mocap.cpp)
+	target_link_libraries(solo_imu_mocap 
+		wolfcore
+		wolfimu
 	    ${PLUGIN_NAME}
 	    ${pinocchio_LIBRARIES}    
 	    /usr/local/lib/libcnpy.so
 	    z
 	)
-	
-	add_executable(solo_mocap_imu solo_mocap_imu.cpp)
-	target_link_libraries(solo_mocap_imu 
-	    ${wolfcore_LIBRARIES}
-	    ${wolfimu_LIBRARIES}
-	    ${PLUGIN_NAME}
-	    ${pinocchio_LIBRARIES}    
-	    /usr/local/lib/libcnpy.so
-	    z
+
+	add_executable(solo_imu_kine_mocap solo_imu_kine_mocap.cpp)
+	target_link_libraries(solo_imu_kine_mocap
+		wolfcore
+		wolfimu
+		${PLUGIN_NAME}
+		${pinocchio_LIBRARIES}    
+		/usr/local/lib/libcnpy.so
+		z
 	)
+
 endif()
 
-add_executable(solo_imu_kine_mocap solo_imu_kine_mocap.cpp)
-target_link_libraries(solo_imu_kine_mocap
-    ${wolfcore_LIBRARIES}
-    ${wolfimu_LIBRARIES}
-    ${PLUGIN_NAME}
-    ${pinocchio_LIBRARIES}    
-    /usr/local/lib/libcnpy.so
-    z
-)
 
 
 
diff --git a/demos/mcapi_utils.cpp b/demos/mcapi_utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d893b4669ce2514f3f15b3bf73500f243923e19c
--- /dev/null
+++ b/demos/mcapi_utils.cpp
@@ -0,0 +1,77 @@
+//--------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 "mcapi_utils.h"
+
+Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr)
+{
+    return ddr + w.cross(dr);
+}
+
+
+std::vector<Vector3d> contacts_from_footrect_center()
+{
+    // compute feet corners coordinates like from the leg_X_6_joint
+    double lx = 0.1;
+    double ly = 0.065;
+    double lz = 0.107;
+    std::vector<Vector3d> contacts; contacts.resize(4);
+    contacts[0] = {-lx, -ly, -lz};
+    contacts[1] = {-lx,  ly, -lz};
+    contacts[2] = { lx, -ly, -lz};
+    contacts[3] = { lx,  ly, -lz};
+    return contacts;
+}
+
+
+Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts, 
+                                 const Matrix<double, 12, 1>& cf12)
+{
+    Vector3d f = cf12.segment<3>(0) + cf12.segment<3>(3) + cf12.segment<3>(6) + cf12.segment<3>(9);
+    Vector3d tau = contacts[0].cross(cf12.segment<3>(0))
+                 + contacts[1].cross(cf12.segment<3>(3))
+                 + contacts[2].cross(cf12.segment<3>(6))
+                 + contacts[3].cross(cf12.segment<3>(9));
+    Matrix<double, 6, 1> wrench; wrench << f, tau;
+    return wrench;
+}
+
+
+Matrix3d compute_Iw(pinocchio::Model& model, 
+                    pinocchio::Data& data, 
+                    VectorXd& q_static, 
+                    Vector3d& b_p_bc)
+{
+    MatrixXd b_Mc = pinocchio::crba(model, data, q_static);  // mass matrix at b frame expressed in b frame
+    // MatrixXd b_Mc = crba(model_dist, data_dist, q_static);  // mass matrix at b frame expressed in b frame
+    MatrixXd b_I_b = b_Mc.topLeftCorner<6,6>();             // inertia matrix at b frame expressed in b frame 
+    pinocchio::SE3 bTc (Eigen::Matrix3d::Identity(), b_p_bc);          // "no free flyer robot -> c frame oriented the same as b"
+    pinocchio::SE3 cTb = bTc.inverse();        
+    // Ic = cXb^star * Ib * bXc = bXc^T * Ib * bXc 
+    // c_I_c block diagonal:
+    // [m*Id3, 03;
+    //  0,     Iw]
+    MatrixXd c_I_c = cTb.toDualActionMatrix() * b_I_b * bTc.toActionMatrix();
+    Matrix3d b_Iw  = c_I_c.bottomRightCorner<3,3>();  // meas
+    return b_Iw;
+}
diff --git a/demos/mcapi_utils.h b/demos/mcapi_utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..c62c4026a1dac41733350f2b96fb554285f30715
--- /dev/null
+++ b/demos/mcapi_utils.h
@@ -0,0 +1,76 @@
+//--------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 <vector>
+#include "Eigen/Dense"
+#include "pinocchio/algorithm/crba.hpp"
+
+
+using namespace Eigen; 
+
+
+/**
+ * \brief Compute a 3D acceleration from 6D spatial acceleration
+ * 
+ * \param ddr spatial acc linear part
+ * \param w spatial acc rotational part
+ * \param dr spatial velocity linear part
+**/
+Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr);
+
+
+/**
+* \brief Compute the relative contact points from foot center of a Talos foot.
+* 
+* Order is clockwise, starting from bottom left (looking at a forward pointing foot from above).
+* Expressed in local foot coordinates. 
+**/
+std::vector<Vector3d> contacts_from_footrect_center();
+
+
+/**
+* \brief Compute the wrench at the end effector center expressed in world coordinates.
+
+* Each contact force (at a foot for instance) is represented in MAPI as a set of 
+* 4 forces at the corners of the contact polygone (foot -> rectangle). These forces,
+* as the other quantities, are expressed in world coordinates. From this 4 3D forces, 
+* we can compute the 6D wrench at the center of the origin point of the contacts vectors.
+
+* \param contacts std vector of 3D contact forces
+* \param cforces 12D corner forces vector ordered as [fx1, fy1, fz1, fx2, fy2... fz4], same order as contacts
+* \param wRl rotation matrix, orientation from world frame to foot frame
+**/
+Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts, 
+                                             const Matrix<double, 12, 1>& cf12);
+
+
+/**
+* \brief Compute the centroidal angular inertia used to compute the angular momentum.
+
+* \param model: pinocchio robot model used
+* \param data: pinocchio robot data used
+* \param q_static: configuration of the robot with free flyer pose at origin (local base frame = world frame)
+* \param b_p_bc: measure of the CoM position relative to the base
+**/
+Matrix3d compute_Iw(pinocchio::Model& model, 
+                    pinocchio::Data& data, 
+                    VectorXd& q_static, 
+                    Vector3d& b_p_bc);
diff --git a/demos/solo_imu_kine_mocap.cpp b/demos/solo_imu_kine_mocap.cpp
index 68c837894f73325895b977784ebba7e5a669801b..82d2df1c42da66a66e6894fe49e037b4a04259cd 100644
--- a/demos/solo_imu_kine_mocap.cpp
+++ b/demos/solo_imu_kine_mocap.cpp
@@ -94,35 +94,35 @@ int main (int argc, char **argv) {
     // retrieve parameters from yaml file
     YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
     double dt = config["dt"].as<double>();
-    double min_t = config["min_t"].as<double>();
+    // double min_t = config["min_t"].as<double>();
     double max_t = config["max_t"].as<double>();
-    double solve_every_sec = config["solve_every_sec"].as<double>();
+    // double solve_every_sec = config["solve_every_sec"].as<double>();
     bool solve_end = config["solve_end"].as<bool>();
 
     // robot
     std::string robot_urdf = config["robot_urdf"].as<std::string>();
-    int dimc = config["dimc"].as<int>();
-    int nb_feet = config["nb_feet"].as<int>();
+    // int dimc = config["dimc"].as<int>();
+    // int nb_feet = config["nb_feet"].as<int>();
 
     // Ceres setup
     int max_num_iterations = config["max_num_iterations"].as<int>();
     bool compute_cov = config["compute_cov"].as<bool>();
     
     // estimator sensor noises
-    double std_pbc_est = config["std_pbc_est"].as<double>();
-    double std_vbc_est = config["std_vbc_est"].as<double>();
-    double std_f_est = config["std_f_est"].as<double>();
-    double std_tau_est = config["std_tau_est"].as<double>();
+    // double std_pbc_est = config["std_pbc_est"].as<double>();
+    // double std_vbc_est = config["std_vbc_est"].as<double>();
+    // double std_f_est = config["std_f_est"].as<double>();
+    // double std_tau_est = config["std_tau_est"].as<double>();
     double std_foot_nomove = config["std_foot_nomove"].as<double>();
     
     // priors
     double std_prior_p = config["std_prior_p"].as<double>();
     double std_prior_o = config["std_prior_o"].as<double>();
     double std_prior_v = config["std_prior_v"].as<double>();
-    double std_abs_bias_pbc = config["std_abs_bias_pbc"].as<double>();
+    // double std_abs_bias_pbc = config["std_abs_bias_pbc"].as<double>();
     double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
     double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();    
-    double std_bp_drift = config["std_bp_drift"].as<double>();
+    // double std_bp_drift = config["std_bp_drift"].as<double>();
     std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
     Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
     std::vector<double> i_p_im_vec = config["i_p_im"].as<std::vector<double>>();
@@ -137,7 +137,7 @@ int main (int argc, char **argv) {
     // contacts
     std::vector<double> l_p_lg_vec = config["l_p_lg"].as<std::vector<double>>();
     Eigen::Map<Vector3d> l_p_lg(l_p_lg_vec.data());
-    double fz_thresh = config["fz_thresh"].as<double>();
+    // double fz_thresh = config["fz_thresh"].as<double>();
     std::vector<double> delta_qa_vec = config["delta_qa"].as<std::vector<double>>();
     Eigen::Map<Eigen::Matrix<double,12,1>> delta_qa(delta_qa_vec.data());
 
@@ -159,7 +159,7 @@ int main (int argc, char **argv) {
     i_q_m.normalize();
     assert(i_q_m.normalized().isApprox(i_q_m));
     SE3 i_T_m(i_q_m, i_p_im);
-    Matrix3d i_R_m =  i_T_m.rotation();
+    // Matrix3d i_R_m =  i_T_m.rotation();
 
     // IMU to MOCAP transform
     SE3 m_T_i = i_T_m.inverse();
@@ -171,7 +171,7 @@ int main (int argc, char **argv) {
     m_q_b.normalize();
     assert(m_q_b.normalized().isApprox(m_q_b));
     SE3 m_T_b(m_q_b, m_p_mb);
-    Matrix3d m_R_b =  m_T_b.rotation();
+    // Matrix3d m_R_b =  m_T_b.rotation();
 
     // IMU to BASE transform (composition)
     SE3 i_T_b = i_T_m*m_T_b;
@@ -320,8 +320,8 @@ int main (int argc, char **argv) {
     Quaterniond w_q_i_init = w_q_m_init*m_q_i;
     VectorComposite x_origin("POV", {w_p_wi_init, w_q_i_init.coeffs(), Vector3d::Zero()});
     VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
-    // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0, 0.0005);  // needed to anchor the problem
-    FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0, 0.0005);  // if mocap used
+    // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0);  // needed to anchor the problem
+    FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0);  // if mocap used
     
     proc_imu->setOrigin(KF1);
 
@@ -367,7 +367,7 @@ int main (int argc, char **argv) {
         VectorComposite curr_state = problem->getState();
         Vector4d o_qvec_i_curr = curr_state['O']; 
         Quaterniond o_q_i_curr(o_qvec_i_curr);
-        Vector3d o_v_oi_curr = curr_state['V']; 
+        // Vector3d o_v_oi_curr = curr_state['V']; 
 
 
         //////////////
@@ -421,7 +421,6 @@ int main (int argc, char **argv) {
         // Or else, retrieve from preprocessed dataset
         Eigen::Map<Matrix<double,12, 1>> l_forces(l_forces_arr+12*i);
 
-        Vector3d o_f_tot = Vector3d::Zero();
         std::vector<Vector3d> l_f_l_vec;
         std::vector<Vector3d> o_f_l_vec;
         std::vector<Vector3d> i_p_il_vec;
@@ -507,7 +506,6 @@ int main (int argc, char **argv) {
             Vector3d Qv_fbk_diag = Qv_fbk.diagonal(); 
             Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal(); 
             Vector6d Qm_fbk_diag; Qm_fbk_diag << Qmp_fbk.diagonal(), Qmo_fbk.diagonal(); 
-            Vector3d Qmo_fbk_diag = Qmo_fbk.diagonal(); 
 
             Qp_fbk_v.push_back(Qp_fbk_diag);
             Qo_fbk_v.push_back(Qo_fbk_diag);
@@ -659,8 +657,6 @@ int main (int argc, char **argv) {
         // std::cout << "Factors " << kf->getTimeStamp().get() << std::endl;
         FactorBasePtrList fac_lst; 
         kf->getFactorList(fac_lst);
-        Vector9d imu_err = Vector9d::Zero();
-        Vector6d pose_err = Vector6d::Zero();
 
         i++;
     }
diff --git a/demos/solo_imu_mocap.cpp b/demos/solo_imu_mocap.cpp
index 273355c0aa1981866ef7c0c309eee2fa73ba8d50..1ffabd4c847bb9f3f0954f5eb70862cc20eedf00 100644
--- a/demos/solo_imu_mocap.cpp
+++ b/demos/solo_imu_mocap.cpp
@@ -134,7 +134,7 @@ int main (int argc, char **argv) {
     m_q_b.normalize();
     assert(m_q_b.normalized().isApprox(m_q_b));
     SE3 m_T_b(m_q_b, m_p_mb);
-    Matrix3d m_R_b =  m_T_b.rotation();
+    // Matrix3d m_R_b =  m_T_b.rotation();
 
     // IMU to BASE transform (composition)
     SE3 i_T_b = i_T_m*m_T_b;
@@ -191,9 +191,9 @@ int main (int argc, char **argv) {
     // double* o_a_oi_arr = o_a_oi_npyarr.data<double>();
     double* i_omg_oi_arr = i_omg_oi_npyarr.data<double>();
     double* qa_arr = qa_npyarr.data<double>();
-    double* dqa_arr = dqa_npyarr.data<double>();
-    double* tau_arr = tau_npyarr.data<double>();
-    double* l_forces_arr = l_forces_npyarr.data<double>();
+    // double* dqa_arr = dqa_npyarr.data<double>();
+    // double* tau_arr = tau_npyarr.data<double>();
+    // double* l_forces_arr = l_forces_npyarr.data<double>();
     // double* w_pose_wm_arr = w_pose_wm_npyarr.data<double>();
     double* m_v_wm_arr = m_v_wm_npyarr.data<double>();
     double* w_v_wm_arr = w_v_wm_npyarr.data<double>();
@@ -273,7 +273,7 @@ int main (int argc, char **argv) {
     Quaterniond w_q_i_init = w_q_m_init*m_q_i;
     VectorComposite x_origin("POV", {w_p_wi_init, w_q_i_init.coeffs(), Vector3d::Zero()});
     VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
-    // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0, 0.0005);
+    // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0);
     FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0);  // if mocap used
     
     proc_imu->setOrigin(KF1);
@@ -366,8 +366,6 @@ int main (int argc, char **argv) {
             // recover covariances at this point
             auto kf_last = problem->getTrajectory()->getLastFrame();
             
-            CaptureBasePtr cap_imu = kf_last->getCaptureOf(sen_imu); 
-
             // compute covariances of KF and capture stateblocks
             Eigen::MatrixXd Qp_fbk =  Eigen::Matrix3d::Identity();
             Eigen::MatrixXd Qo_fbk =  Eigen::Matrix3d::Identity();  // global or local?
@@ -399,7 +397,6 @@ int main (int argc, char **argv) {
             Vector3d Qv_fbk_diag = Qv_fbk.diagonal(); 
             Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal(); 
             Vector6d Qm_fbk_diag; Qm_fbk_diag << Qmp_fbk.diagonal(), Qmo_fbk.diagonal(); 
-            Vector3d Qmo_fbk_diag = Qmo_fbk.diagonal(); 
 
             Qp_fbk_v.push_back(Qp_fbk_diag);
             Qo_fbk_v.push_back(Qo_fbk_diag);
diff --git a/demos/solo_real_povcdl_estimation.cpp b/demos/solo_real_povcdl_estimation.cpp
index 8ba3ea20404fe8b1dc8e0ad2cb51131c08a5710e..9953db41444d0d1b1da732f5460c3bd398cd3a63 100644
--- a/demos/solo_real_povcdl_estimation.cpp
+++ b/demos/solo_real_povcdl_estimation.cpp
@@ -69,7 +69,6 @@
 #include "bodydynamics/sensor/sensor_point_feet_nomove.h"
 #include "bodydynamics/processor/processor_point_feet_nomove.h"
 
-// UTILS
 #include "mcapi_utils.h"
 
 
@@ -88,7 +87,7 @@ int main (int argc, char **argv) {
     int dimc = config["dimc"].as<int>();
     double mass_offset = config["mass_offset"].as<double>();
     double dt = config["dt"].as<double>();
-    double min_t = config["min_t"].as<double>();
+    // double min_t = config["min_t"].as<double>();
     double max_t = config["max_t"].as<double>();
     double solve_every_sec = config["solve_every_sec"].as<double>();
     bool solve_end = config["solve_end"].as<bool>();
@@ -334,7 +333,7 @@ int main (int argc, char **argv) {
     // 1. Call setOrigin processorIMU  to generate a fake captureIMU -> generates b_imu
     // 2. process one IKin capture corresponding to the same KF, the factor needing b_imu -> generates a b_pbc
     // 3. set processorForceTorquePreint origin which requires both b_imu and b_pbc
-    proc_inkin->keyFrameCallback(KF1, 0.0005);
+    proc_inkin->keyFrameCallback(KF1);
     proc_imu->setOrigin(KF1);
     // proc_legodom->setOrigin(KF1);
 
@@ -577,8 +576,9 @@ int main (int argc, char **argv) {
             }
         }
 
-        CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
-        CPF->process();
+        // TODO: ???? kin_incontact is a map to Vector3d in this implementation, should be Vector7d [p, q]
+        // CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
+        // CPF->process();
 
 
 
diff --git a/include/bodydynamics/factor/factor_point_feet_nomove.h b/include/bodydynamics/factor/factor_point_feet_nomove.h
index 1e92c70c10ba0e9987b3f265129a3118038090da..9e17e16b75f6cc703ea6f5f7c58b35df27b39aad 100644
--- a/include/bodydynamics/factor/factor_point_feet_nomove.h
+++ b/include/bodydynamics/factor/factor_point_feet_nomove.h
@@ -158,7 +158,8 @@ bool FactorPointFeetNomove::operator () (
     Eigen::Quaternion<T> bm_q_l(bm_qvec_l);
     Eigen::Quaternion<T> b_q_l(b_qvec_l);
 
-    double radius = std::dynamic_pointer_cast<SensorPointFeetNomove>(getSensor())->getFootRadius();
+    // double radius = std::dynamic_pointer_cast<SensorPointFeetNomove>(getSensor())->getFootRadius();  // DOES NOT COMPILE
+    double radius = 0.0;  // deactivate, was not doing so great anyway
 
     // If the radius of the foot is not negligeable, try to account for it with a more complex model
     Matrix<T,3,1> err;
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_point_feet_nomove.cpp b/src/processor/processor_point_feet_nomove.cpp
index 041a6f094d1e80a556d10e70272133c80dd31760..161c16dd18e4bdd512868d6c9bc76ed2d4f87442 100644
--- a/src/processor/processor_point_feet_nomove.cpp
+++ b/src/processor/processor_point_feet_nomove.cpp
@@ -61,11 +61,11 @@ inline void ProcessorPointFeetNomove::processCapture(CaptureBasePtr _capture)
     incoming_ = _capture;
 
     // nothing to do if any of the two buffer is empty
-    if(buffer_pack_kf_.empty()){
+    if(buffer_frame_.empty()){
         WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ",  _capture->getTimeStamp());
         return;
     }
-    if(buffer_pack_kf_.empty()){
+    if(buffer_frame_.empty()){
         WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ",  _capture->getTimeStamp());
         return;
     }
@@ -75,7 +75,7 @@ inline void ProcessorPointFeetNomove::processCapture(CaptureBasePtr _capture)
 
 }
 
-inline void ProcessorPointFeetNomove::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
+inline void ProcessorPointFeetNomove::processKeyFrame(FrameBasePtr _keyframe_ptr)
 {
     if (!_keyframe_ptr)
     {
@@ -83,11 +83,11 @@ inline void ProcessorPointFeetNomove::processKeyFrame(FrameBasePtr _keyframe_ptr
         return;
     }
     // nothing to do if any of the two buffer is empty
-    if(buffer_pack_kf_.empty()){
+    if(buffer_frame_.empty()){
         WOLF_DEBUG("ProcessorPointFeetNomove: KF pack buffer empty, time ",  _keyframe_ptr->getTimeStamp());
         return;
     }
-    if(buffer_pack_kf_.empty()){
+    if(buffer_frame_.empty()){
         WOLF_DEBUG("ProcessorPointFeetNomove: Capture buffer empty, time ",  _keyframe_ptr->getTimeStamp());
         return;
     }
@@ -112,10 +112,10 @@ void ProcessorPointFeetNomove::processKeyFrameCreateFactorFar(){
 
     auto sensor_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(getSensor());
 
-    for (auto kf_pk_it: buffer_pack_kf_.getContainer()){
-        TimeStamp ts = kf_pk_it.first;
-        auto kf_pk = kf_pk_it.second;
-        auto cap_it = buffer_capture_.selectIterator(ts, kf_pk->time_tolerance);
+    for (auto kf_it: buffer_frame_.getContainer()){
+        TimeStamp ts = kf_it.first;
+        auto kf = kf_it.second;
+        auto cap_it = buffer_capture_.selectIterator(ts, params_->time_tolerance);
 
         // We have a match between the KF and a capture from the capture buffer
         if (cap_it != buffer_capture_.getContainer().end()){
@@ -129,13 +129,13 @@ void ProcessorPointFeetNomove::processKeyFrameCreateFactorFar(){
                     // if not, define current KF as the leg track origin and link the capture to this KF
                     // Error occurs if we try to link several time the same Cap->kf due to the loop on legs -> do only once 
                     if (!cap_curr->getFrame()){
-                        cap_curr->link(kf_pk->key_frame);
+                        cap_curr->link(kf);
                     }
                     tracks_[leg_id] = cap_curr;
                 }
                 else {
                     if (!cap_curr->getFrame()){
-                        cap_curr->link(kf_pk->key_frame);
+                        cap_curr->link(kf);
                     }
                     // if it has, create a factor
                     auto cap_origin = std::static_pointer_cast<CapturePointFeetNomove>(tracks_[leg_id]);
@@ -149,12 +149,12 @@ void ProcessorPointFeetNomove::processKeyFrameCreateFactorFar(){
                     // Zero Altitude factor
                     Vector4d meas_altitude; meas_altitude << kin.second.head<3>(), 0;
                     Matrix1d alt_cov = pow(0.01, 2)*Matrix1d::Identity(); 
-                    CaptureBasePtr cap_alt = CaptureBase::emplace<CaptureBase>(kf_pk->key_frame, "Alt0", cap_curr->getTimeStamp());
+                    CaptureBasePtr cap_alt = CaptureBase::emplace<CaptureBase>(kf, "Alt0", cap_curr->getTimeStamp());
                     FeatureBasePtr feat_alt = FeatureBase::emplace<FeatureBase>(cap_alt, "Alt0", meas_altitude, alt_cov);
                     FactorBasePtr fac_alt = FactorBase::emplace<FactorPointFeetAltitude>(feat_alt, feat_alt, nullptr, true);
                 }
             }
-            buffer_pack_kf_.removeUpTo(cap_curr->getTimeStamp());
+            buffer_frame_.removeUpTo(cap_curr->getTimeStamp());
             buffer_capture_.removeUpTo(cap_curr->getTimeStamp());
         }
     }
@@ -227,12 +227,12 @@ void ProcessorPointFeetNomove::createFactorConsecutive(){
                     double dt_kf = cap2->getTimeStamp() - cap1->getTimeStamp();
                     Matrix3d cov_int = dt_kf*sensor_pfnm->getCovFootNomove();  // integrate zero velocity cov during dt_kf
                     FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap2, "PointFeet", meas, cov_int);
-                    FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, kf1_it->second->key_frame, nullptr, true);
+                    FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, kf1_it->second, nullptr, true);
 
                     // Zero Altitude factor
                     Vector4d meas_altitude; meas_altitude << kin_pair2_it->second.head<3>(), 0;
                     Matrix1d alt_cov = pow(0.01, 2)*Matrix1d::Identity(); 
-                    CaptureBasePtr cap_alt = CaptureBase::emplace<CaptureBase>(kf2_it->second->key_frame, "Alt0", cap2->getTimeStamp());
+                    CaptureBasePtr cap_alt = CaptureBase::emplace<CaptureBase>(kf2_it->second, "Alt0", cap2->getTimeStamp());
                     FeatureBasePtr feat_alt = FeatureBase::emplace<FeatureBase>(cap_alt, "Alt0", meas_altitude, alt_cov);
                     FactorBasePtr fac_alt = FactorBase::emplace<FactorPointFeetAltitude>(feat_alt, feat_alt, nullptr, true);
                 }
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 60f23c1fe3f823d3c1c848365735928550d24095..5850e2db6763d74e6385f16417ed48102091ba5b 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -1,66 +1,33 @@
 # Retrieve googletest from github & compile
 add_subdirectory(gtest)
 
-
-## Added these two include_dirs: ######################
-#
-#CMAKE modules
-#SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
-#MESSAGE(STATUS ${CMAKE_MODULE_PATH})
-#
-# Include Eigen
-#FIND_PACKAGE(Eigen 3 REQUIRED)
-#INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-#
-# Include Ceres
-#FIND_PACKAGE(Ceres QUIET) #Ceres is not required
-#IF(Ceres_FOUND)
-#    INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
-#ENDIF(Ceres_FOUND)
-#
-## and now gtest_motion_2d works ######################
-
-
-# Include gtest directory.
-include_directories(${GTEST_INCLUDE_DIRS})
-
-################# ADD YOUR TESTS BELOW ####################
+############# USE THIS TEST AS AN EXAMPLE #################
 #                                                         #
-#           ==== IN ALPHABETICAL ORDER! ====              #
+# Create a specific test executable for gtest_example     #
+# wolf_add_gtest(gtest_example gtest_example.cpp)         #
 #                                                         #
+###########################################################
+
 wolf_add_gtest(gtest_capture_inertial_kinematics gtest_capture_inertial_kinematics.cpp)
-target_link_libraries(gtest_capture_inertial_kinematics ${PLUGIN_NAME})
 
 wolf_add_gtest(gtest_capture_leg_odom gtest_capture_leg_odom.cpp)
-target_link_libraries(gtest_capture_leg_odom ${PLUGIN_NAME})
 
 wolf_add_gtest(gtest_feature_inertial_kinematics gtest_feature_inertial_kinematics.cpp)
-target_link_libraries(gtest_feature_inertial_kinematics ${PLUGIN_NAME})
 
 wolf_add_gtest(gtest_factor_inertial_kinematics gtest_factor_inertial_kinematics.cpp)
-target_link_libraries(gtest_factor_inertial_kinematics ${PLUGIN_NAME})
 
 wolf_add_gtest(gtest_factor_force_torque gtest_factor_force_torque.cpp)
-target_link_libraries(gtest_factor_force_torque ${PLUGIN_NAME})
 
 wolf_add_gtest(gtest_force_torque_delta_tools gtest_force_torque_delta_tools.cpp)
-target_link_libraries(gtest_force_torque_delta_tools ${PLUGIN_NAME})
 
+# TODO: revive those
 # wolf_add_gtest(gtest_feature_force_torque_preint gtest_feature_force_torque_preint.cpp)
-# target_link_libraries(gtest_feature_force_torque_preint ${PLUGIN_NAME})
-
 # wolf_add_gtest(gtest_processor_inertial_kinematics gtest_processor_inertial_kinematics.cpp)
-# target_link_libraries(gtest_processor_inertial_kinematics ${PLUGIN_NAME})
-
 # wolf_add_gtest(gtest_processor_force_torque_preint gtest_processor_force_torque_preint.cpp)
-# target_link_libraries(gtest_processor_force_torque_preint ${PLUGIN_NAME})
 
 wolf_add_gtest(gtest_processor_point_feet_nomove gtest_processor_point_feet_nomove.cpp)
-target_link_libraries(gtest_processor_point_feet_nomove ${PLUGIN_NAME})
 
 wolf_add_gtest(gtest_sensor_force_torque gtest_sensor_force_torque.cpp)
-target_link_libraries(gtest_sensor_force_torque ${PLUGIN_NAME})
 
 wolf_add_gtest(gtest_sensor_inertial_kinematics gtest_sensor_inertial_kinematics.cpp)
-target_link_libraries(gtest_sensor_inertial_kinematics ${PLUGIN_NAME})
 
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
index 559756b02472653c655c44152c452ba8767ef6f2..2294c819aec95c1f2c6bed5c0e7b1d6cc857c614 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} PUBLIC libgtest ${PLUGIN_NAME})
+    target_include_directories(${target} PUBLIC ${GTEST_INCLUDE_DIRS})
+  else()
+    target_link_libraries(${target} PUBLIC gtest_main ${PLUGIN_NAME})
+  endif()
   add_test(NAME ${target} COMMAND ${target})
 endfunction()