diff --git a/CMakeLists.txt b/CMakeLists.txt
index e9e3f77c36652f9c83b95a31e9f138d27b1a13a0..f62d07a3a206408d11bcaea4df4f3fe5d44a8d51 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -233,14 +233,15 @@ SET(HDRS_MATH
   include/core/math/rotations.h
   )
 SET(HDRS_UTILS
+  include/core/utils/converter.h
   include/core/utils/eigen_assert.h
   include/core/utils/eigen_predicates.h
+  include/core/utils/loader.hpp
   include/core/utils/logging.h
   include/core/utils/make_unique.h
-  include/core/utils/singleton.h
   include/core/utils/params_server.hpp
-  include/core/utils/converter.h
-  include/core/utils/loader.hpp
+  include/core/utils/singleton.h
+  include/core/utils/utils_gtest.h
   )
 SET(HDRS_PROBLEM
   include/core/problem/problem.h
diff --git a/codetemplates eclipse.xml b/codetemplates eclipse.xml
index a83cc56bd43bb272eff012dd89a401c33283610f..8fb5a247ead13c2795a1a7d70da79187d010ad7b 100644
--- a/codetemplates eclipse.xml	
+++ b/codetemplates eclipse.xml	
@@ -23,7 +23,7 @@ ${declarations}
 
 ${namespace_end}</template><template autoinsert="false" context="org.eclipse.cdt.core.cxxSource.contenttype_context" deleted="false" description="Default template for newly created C++ test files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.cpptestfile" name="Default C++ test template">${filecomment}
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 ${includes}
 
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
index a6146b6615adcd9bea24d3c12fab7d70ee013a12..c922ae4b0b6e80ddcf75090d09bcf04d58406e1e 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.cpp
@@ -7,7 +7,7 @@
 
 
 #include "core/math/SE3.h"
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 
 
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 8bb84231d13a800845d80703025e2c382d187f8b..633669c7b4a5fea47832bf22ab009cb3671cad7a 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -5,7 +5,7 @@
  *      Author: jsola
  */
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 #include "core/capture/capture_base.h"
 #include "core/state_block/state_angle.h"
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 4ab28374d25f09b63cceb7fa709ddfa9367ec347..28cd36ced23e26ab9921905c7227640e9bbf1134 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -5,7 +5,7 @@
  *      Author: jvallve
  */
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 #include "core/utils/logging.h"
 
 #include "core/problem/problem.h"
diff --git a/test/gtest_converter.cpp b/test/gtest_converter.cpp
index 69bb7ce355646daadaf3b91a1e816e0f0c163587..b49f30a2d3e3f519dcc9e525aa8652830c809b51 100644
--- a/test/gtest_converter.cpp
+++ b/test/gtest_converter.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 #include "core/utils/converter.h"
 
 using namespace std;
diff --git a/test/gtest_eigen_predicates.cpp b/test/gtest_eigen_predicates.cpp
index c7c68b4f3a2df00fab257fb4f47646d429f361be..0207f811e5e3797571905a4330e2d3f5a1a63371 100644
--- a/test/gtest_eigen_predicates.cpp
+++ b/test/gtest_eigen_predicates.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 #include "core/utils/eigen_predicates.h"
 
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index 92e42e4405b0cdd427af740a239849cdb8a53079..cf6ae9004d91a009c87768373f5974c27c879086 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -5,7 +5,7 @@
  *      Author: jcasals
  */
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 #include "core/utils/logging.h"
 
 #include "core/problem/problem.h"
diff --git a/test/gtest_example.cpp b/test/gtest_example.cpp
index 0cadea4ffe640770290f82ea4b5e17542468d404..acfe5d6b72fa8bedf739497883b047d44efdd854 100644
--- a/test/gtest_example.cpp
+++ b/test/gtest_example.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 TEST(TestTest, DummyTestExample)
 {
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index da1cc8c050276e2e5f349773493027d0a96b0111..af452608f8a602ad2128ea5c44b81cf832b6004b 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -5,7 +5,7 @@
  *      \author: datchuth
  */
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 #include "core/factor/factor_block_absolute.h"
 #include "core/factor/factor_quaternion_absolute.h"
 #include "core/capture/capture_motion.h"
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index efa61acc9cc027c965aca8f8f27ac7f28e2fde58..108ae75cdb54fa50a3caf5eaa9733a096b893578 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -5,7 +5,7 @@
  *      Author: jvallve
  */
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 #include "core/sensor/sensor_odom_2D.h"
 #include "core/capture/capture_void.h"
diff --git a/test/gtest_factor_autodiff_distance_3D.cpp b/test/gtest_factor_autodiff_distance_3D.cpp
index c605a558d48b12a0f4dad46ad718f929f1bbc62c..6fb4c644e4abc8045d1c842d533e5a73286cb431 100644
--- a/test/gtest_factor_autodiff_distance_3D.cpp
+++ b/test/gtest_factor_autodiff_distance_3D.cpp
@@ -11,7 +11,7 @@
 #include "core/ceres_wrapper/ceres_manager.h"
 #include "core/math/rotations.h"
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 using namespace wolf;
 using namespace Eigen;
diff --git a/test/gtest_factor_odom_3D.cpp b/test/gtest_factor_odom_3D.cpp
index 09ae42b0c6f523f8a0ed98f49c570c82cbec9d7e..7c239323a93e009a7e8f4c71eca650022ee2cd94 100644
--- a/test/gtest_factor_odom_3D.cpp
+++ b/test/gtest_factor_odom_3D.cpp
@@ -5,7 +5,7 @@
  *      \author: jsola
  */
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 #include "core/factor/factor_odom_3D.h"
 #include "core/capture/capture_motion.h"
diff --git a/test/gtest_factor_pose_2D.cpp b/test/gtest_factor_pose_2D.cpp
index bc50e62780e9812cbcdab2ffbe03a927ad3a650f..c28a8946ef393b2e41de96d424108dd44691d84a 100644
--- a/test/gtest_factor_pose_2D.cpp
+++ b/test/gtest_factor_pose_2D.cpp
@@ -6,7 +6,7 @@
  */
 
 #include "core/factor/factor_pose_2D.h"
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 #include "core/capture/capture_motion.h"
 
diff --git a/test/gtest_factor_pose_3D.cpp b/test/gtest_factor_pose_3D.cpp
index 45ee64674e3988bfb6a2c0a0c2c48c83fc0b438e..eda34b179b9d579cfc535dc3d488c5fef74cbc7b 100644
--- a/test/gtest_factor_pose_3D.cpp
+++ b/test/gtest_factor_pose_3D.cpp
@@ -6,7 +6,7 @@
  */
 
 #include "core/factor/factor_pose_3D.h"
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 #include "core/capture/capture_motion.h"
 
diff --git a/test/gtest_factor_sparse.cpp b/test/gtest_factor_sparse.cpp
index 6c8f212be2030c5d04707eb5b5798848f36d8710..53d8b91a196989936c9d908955ed2b1bc7c73eda 100644
--- a/test/gtest_factor_sparse.cpp
+++ b/test/gtest_factor_sparse.cpp
@@ -5,7 +5,7 @@
  *      \author: jsola
  */
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 #include "factor_sparse.h"
 
diff --git a/test/gtest_feature_base.cpp b/test/gtest_feature_base.cpp
index 3a93ef2cb46396defc8f3b3f541042ab85e4a1a8..713c044e2684e088606a99b02d3009ce35239f9e 100644
--- a/test/gtest_feature_base.cpp
+++ b/test/gtest_feature_base.cpp
@@ -7,7 +7,7 @@
 
 #include "core/feature/feature_base.h"
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 using namespace wolf;
 using namespace Eigen;
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 4cbc927bfe4e67fb4464c10aad4b1596c45e0987..4f189f559920003b3a0b8a033a56cde340d997ef 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -5,7 +5,7 @@
  *      Author: jsola
  */
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 #include "core/utils/logging.h"
 
 #include "core/frame/frame_base.h"
diff --git a/test/gtest_local_param.cpp b/test/gtest_local_param.cpp
index 520a694f6622d4601bfa2689422dbed35a2ef413..267d912b6f8d8499b2e956b3bf90f2a5a7b95be5 100644
--- a/test/gtest_local_param.cpp
+++ b/test/gtest_local_param.cpp
@@ -5,7 +5,7 @@
  *      author: jsola
  */
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 #include "core/utils/logging.h"
 
 #include "core/state_block/local_parametrization_quaternion.h"
diff --git a/test/gtest_make_posdef.cpp b/test/gtest_make_posdef.cpp
index 5a0b707a6487eda22e37f2601408cda64929015d..18222fd3b3ec94e009ec649a11730db1a95d4422 100644
--- a/test/gtest_make_posdef.cpp
+++ b/test/gtest_make_posdef.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 #include "core/common/wolf.h"
 
 using namespace Eigen;
diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp
index 3ef0bfaa050b6125427b2b16256c56719f54fbaf..8bd5f665921b35b0087d4636e86cd74691869cda 100644
--- a/test/gtest_motion_buffer.cpp
+++ b/test/gtest_motion_buffer.cpp
@@ -5,7 +5,7 @@
  *      Author: jsola
  */
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 #include "core/utils/logging.h"
 
 #include "core/processor/motion_buffer.h"
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index faa22aff39d4af1878f051e6687a7a327e789f67..a25cf1733f6f78f43bb717613956f38306bbfd1b 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -5,7 +5,7 @@
  *      \author: jsola
  */
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 // Classes under test
 #include "core/processor/processor_odom_2D.h"
diff --git a/test/gtest_odom_3D.cpp b/test/gtest_odom_3D.cpp
index ce52ec3932b0162ca400cff88680ea27b7b4b196..3c13449cede9e839e14c50e47d7db78e2134dc1d 100644
--- a/test/gtest_odom_3D.cpp
+++ b/test/gtest_odom_3D.cpp
@@ -5,7 +5,7 @@
  *      \author: jsola
  */
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 #include "core/common/wolf.h"
 #include "core/utils/logging.h"
diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
index c2b8958d0ca9170f4ea317404b0c0cf2ccb9457f..6597f0f53207c98759e882a96221a2ac0fcb00d0 100644
--- a/test/gtest_pack_KF_buffer.cpp
+++ b/test/gtest_pack_KF_buffer.cpp
@@ -5,7 +5,7 @@
  *      Author: jsola
  */
 //Wolf
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 #include "core/processor/processor_odom_2D.h"
 #include "core/sensor/sensor_odom_2D.h"
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index e5ddb8653a7b8e8246c1a2e8707c6eb186e66ae5..177961d922ebf117b0a696fd870cfe76f48b86ad 100644
--- a/test/gtest_param_prior.cpp
+++ b/test/gtest_param_prior.cpp
@@ -5,7 +5,7 @@
  *      Author: jvallve
  */
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 #include "core/utils/logging.h"
 
 #include "core/problem/problem.h"
diff --git a/test/gtest_param_server.cpp b/test/gtest_param_server.cpp
index a24b07a7e01b8a37b17806fd89d25d10a5e8f22b..3e83f8f89955a57342a0ec5247afb63303de69ce 100644
--- a/test/gtest_param_server.cpp
+++ b/test/gtest_param_server.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 #include "core/utils/converter.h"
 #include "core/common/wolf.h"
 #include "core/yaml/parser_yaml.hpp"
diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp
index 752c06fd29831bb29dcd550ed35cdc24bd76d3de..97451a6060ccd3adb43fd990c929c9cfee7b8dbc 100644
--- a/test/gtest_parser_yaml.cpp
+++ b/test/gtest_parser_yaml.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 #include "core/utils/converter.h"
 #include "core/common/wolf.h"
 #include "core/yaml/parser_yaml.hpp"
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index e25ba4baef1ff16bb61fcb399c9c7c6d35744700..44fac86d12c9134180c83896d6906ee954a65abf 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -5,7 +5,7 @@
  *      Author: jsola
  */
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 #include "core/utils/logging.h"
 
 #include "core/problem/problem.h"
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 9128e1ee66c9a1442a87a2998ec43ba29878f2bf..cff4bc083e1fa349e7986df8d6e4af1bb56b15f6 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -6,7 +6,7 @@
  */
 
 //Wolf
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 #include "core/processor/processor_odom_2D.h"
 #include "core/sensor/sensor_odom_2D.h"
diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
index 95b16425310926f60e6fbe29d4ce16629d90f55e..82b3d2ae02e1e1821b9dca28b35792f6855a8f5e 100644
--- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
+++ b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
@@ -6,7 +6,7 @@
  *      \author: tessajohanna
  */
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 #include "core/utils/logging.h"
 
 #include "core/sensor/sensor_odom_2D.h"
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 60d175a6d3d6ee66f58b4549f65f73f068b83703..57d7a5c685f6e60aa13d57829e8f83b9afcb3233 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -5,7 +5,7 @@
  *      Author: jsola
  */
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 #include "core/common/wolf.h"
 #include "core/utils/logging.h"
diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp
index 513cddd55abc972f4f06df308a2eb35becffd366..35335523c9d3fbe02e8801f4cc88a4cbe8f747b4 100644
--- a/test/gtest_rotation.cpp
+++ b/test/gtest_rotation.cpp
@@ -18,7 +18,7 @@
 #include <iomanip>
 #include <ctime>
 #include <cmath>
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 //#define write_results
 
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index d8420b2b3fa2d9635149c28b34ce3853bd63ca1a..499629f8fc934eaa5853670dd7d24a69fdc8885c 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -7,7 +7,7 @@
 
 #include "core/sensor/sensor_base.h"
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 using namespace wolf;
 
diff --git a/test/gtest_shared_from_this.cpp b/test/gtest_shared_from_this.cpp
index 6b14fff0c5b9db4d16f8c0e9b22393157387fd9d..b2d5217f4be5f08525da8042478b32c871a7afa8 100644
--- a/test/gtest_shared_from_this.cpp
+++ b/test/gtest_shared_from_this.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 #include "core/common/node_base.h"
 
 class CChildBase;
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 92fc07b84ba8228a4c1b3ef90790a35dd28405b1..815817a6eac6faabf045a438819d09ae2d2529be 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -5,7 +5,7 @@
  *      Author: jvallve
  */
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 #include "core/utils/logging.h"
 
 #include "core/problem/problem.h"
diff --git a/test/gtest_time_stamp.cpp b/test/gtest_time_stamp.cpp
index 03b1d0582280f294d1fbe5bd217b8294f861ca73..7a39a5a6d0e794b8745339442b4959a6be76d8f6 100644
--- a/test/gtest_time_stamp.cpp
+++ b/test/gtest_time_stamp.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 #include "core/common/time_stamp.h"
 
 #include <thread>
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 2f956b0846a505568c084147a1121904cc2a80da..ff930b512b8f8da960b3921f81bc2bd73eac03b5 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -5,7 +5,7 @@
  *      \author: jsola
  */
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 #include "core/processor/track_matrix.h"
 
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index f71bc579b49ced7d6e389d917c690eed06e00566..2edffe94d62ca65ce10bce645eeb5d08d685676f 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -5,7 +5,7 @@
  *      Author: jsola
  */
 
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 #include "core/utils/logging.h"
 
 #include "core/problem/problem.h"
diff --git a/test/utils_gtest.h b/test/utils_gtest.h
deleted file mode 100644
index 9cb2166e8429cd236f77c502891c14880e57ec30..0000000000000000000000000000000000000000
--- a/test/utils_gtest.h
+++ /dev/null
@@ -1,146 +0,0 @@
-/**
- * \file utils_gtest.h
- * \brief Some utils for gtest
- * \author Jeremie Deray
- *  Created on: 26/09/2016
- *  Eigen macros extension by: Joan Sola on 26/04/2017
- */
-
-#ifndef WOLF_UTILS_GTEST_H
-#define WOLF_UTILS_GTEST_H
-
-#include <gtest/gtest.h>
-
-// Macros for testing equalities and inequalities.
-//
-//    * {ASSERT|EXPECT}_EQ(expected, actual): Tests that expected == actual
-//    * {ASSERT|EXPECT}_NE(v1, v2):           Tests that v1 != v2
-//    * {ASSERT|EXPECT}_LT(v1, v2):           Tests that v1 < v2
-//    * {ASSERT|EXPECT}_LE(v1, v2):           Tests that v1 <= v2
-//    * {ASSERT|EXPECT}_GT(v1, v2):           Tests that v1 > v2
-//    * {ASSERT|EXPECT}_GE(v1, v2):           Tests that v1 >= v2
-//
-// C String Comparisons.  All tests treat NULL and any non-NULL string
-// as different.  Two NULLs are equal.
-//
-//    * {ASSERT|EXPECT}_STREQ(s1, s2):     Tests that s1 == s2
-//    * {ASSERT|EXPECT}_STRNE(s1, s2):     Tests that s1 != s2
-//    * {ASSERT|EXPECT}_STRCASEEQ(s1, s2): Tests that s1 == s2, ignoring case
-//    * {ASSERT|EXPECT}_STRCASENE(s1, s2): Tests that s1 != s2, ignoring case
-//
-// Macros for comparing floating-point numbers.
-//
-//    * {ASSERT|EXPECT}_FLOAT_EQ(expected, actual):
-//         Tests that two float values are almost equal.
-//    * {ASSERT|EXPECT}_DOUBLE_EQ(expected, actual):
-//         Tests that two double values are almost equal.
-//    * {ASSERT|EXPECT}_NEAR(v1, v2, abs_error):
-//         Tests that v1 and v2 are within the given distance to each other.
-//
-// These predicate format functions work on floating-point values, and
-// can be used in {ASSERT|EXPECT}_PRED_FORMAT2*(), e.g.
-//
-//   EXPECT_PRED_FORMAT2(testing::DoubleLE, Foo(), 5.0);
-//
-// Macros that execute statement and check that it doesn't generate new fatal
-// failures in the current thread.
-//
-//    * {ASSERT|EXPECT}_NO_FATAL_FAILURE(statement);
-
-// http://stackoverflow.com/a/29155677
-
-namespace testing
-{
-namespace internal
-{
-enum GTestColor
-{
-  COLOR_DEFAULT,
-  COLOR_RED,
-  COLOR_GREEN,
-  COLOR_YELLOW
-};
-
-extern void ColoredPrintf(GTestColor color, const char* fmt, ...);
-
-#define PRINTF(...) \
-  do { testing::internal::ColoredPrintf(testing::internal::COLOR_GREEN,\
-  "[          ] "); \
-  testing::internal::ColoredPrintf(testing::internal::COLOR_YELLOW, __VA_ARGS__); } \
-  while(0)
-
-#define PRINT_TEST_FINISHED \
-{ \
-  const ::testing::TestInfo* const test_info = \
-    ::testing::UnitTest::GetInstance()->current_test_info(); \
-  PRINTF(std::string("Finished test case ").append(test_info->test_case_name()).\
-          append(" of test ").append(test_info->name()).append(".\n").c_str()); \
-}
-
-// C++ stream interface
-class TestCout : public std::stringstream
-{
-public:
-  ~TestCout()
-  {
-    PRINTF("%s\n", str().c_str());
-  }
-};
-
-/* Usage :
-
-TEST(Test, Foo)
-{
-  // the following works but prints default stream
-  EXPECT_TRUE(false) << "Testing Stream.";
-
-  // or you can play with AINSI color code
-  EXPECT_TRUE(false) << "\033[1;31m" << "Testing Stream.";
-
-  // or use the above defined macros
-
-  PRINTF("Hello world");
-
-  // or
-
-  TEST_COUT << "Hello world";
-}
-
-*/
-#define TEST_COUT testing::internal::TestCout()
-
-} // namespace internal
-
-/* Macros related to testing Eigen classes:
- */
-#define EXPECT_MATRIX_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
-                  return (lhs - rhs).isMuchSmallerThan(1, precision); \
-               }, \
-               C_expect, C_actual);
-
-#define ASSERT_MATRIX_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
-                  return (lhs - rhs).isMuchSmallerThan(1, precision); \
-               }, \
-               C_expect, C_actual);
-
-#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
-
-#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
-
-#define EXPECT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
-                   MatrixXs er = lhs - rhs; \
-                   er(2) = pi2pi((Scalar)er(2)); \
-                   return er.isMuchSmallerThan(1, precision); \
-               }, \
-               C_expect, C_actual);
-
-#define ASSERT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
-                   MatrixXs er = lhs - rhs; \
-                   er(2) = pi2pi((Scalar)er(2)); \
-                   return er.isMuchSmallerThan(1, precision); \
-               }, \
-               C_expect, C_actual);
-
-} // namespace testing
-
-#endif /* WOLF_UTILS_GTEST_H */
diff --git a/wolf_scripts/templates/gtest_template.cpp b/wolf_scripts/templates/gtest_template.cpp
index c797c1b5ad21d8c09ce89d3cb116cb6da299bacb..ccfec27db00cba6d92ac7f5d6b91718db1896be2 100644
--- a/wolf_scripts/templates/gtest_template.cpp
+++ b/wolf_scripts/templates/gtest_template.cpp
@@ -1,4 +1,4 @@
-#include "utils_gtest.h"
+#include "core/utils/utils_gtest.h"
 
 #include "wolf.h"
 #include "logging.h"
diff --git a/wolf_scripts/wolf_update.sh b/wolf_scripts/wolf_update.sh
index 1c7d448740d5f65909082cc429346c2c9824406a..7a0ceac5be717ff17fbe5acab186fda636de7ec4 100755
--- a/wolf_scripts/wolf_update.sh
+++ b/wolf_scripts/wolf_update.sh
@@ -29,7 +29,7 @@ for folder in wolf IMU laser gnss vision apriltag; do
             if ! cmake ..; then
                 echo ""
                 echo "==========================================================================================================================="
-                echo "${RED} CMake has failed, avorting process for $folder ${NC}"
+                echo "${YELLOW} CMake has failed, avorting process for $folder ${NC}"
                 echo "==========================================================================================================================="
                 echo ""
                 sleep 1