diff --git a/CMakeLists.txt b/CMakeLists.txt
index b0da1a80eb594eb158b4901a1ceb7f0c026fabc9..89e95670ed1f2a794ddf689e432f0f4c5e342e69 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -39,8 +39,21 @@ endif()
 
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fpermissive")
 
+IF(NOT BUILD_TESTS)
+  OPTION(BUILD_TESTS "Build Unit tests" ON)
+ENDIF(NOT BUILD_TESTS)
+
 ADD_SUBDIRECTORY(src)
 
+if(BUILD_TESTS)
+	# Enables testing for this directory and below.
+    # Note that ctest expects to find a test file in the build directory root.
+    # Therefore, this command should be in the source directory root.
+    #include(CTest) # according to http://public.kitware.com/pipermail/cmake/2012-June/050853.html
+    enable_testing()
+  	MESSAGE("Building tests.")
+endif()
+
 FIND_PACKAGE(Doxygen)
 
 FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
@@ -81,4 +94,4 @@ ELSE(UNIX)
     COMMENT "uninstall only implemented in unix"
     TARGET  uninstall
   )
-ENDIF(UNIX)
+ENDIF(UNIX)
\ No newline at end of file
diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index 1a555b0d3a53041434a567705bf164a8e44e7ec4..e90e6cc671916de3f6020fc17afd33d9d7600f9e 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/gnss_utils.h
@@ -56,8 +56,13 @@ namespace GNSSUtils
                 const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
                 double *resp, char *msg);
 
-  Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef);
-  Eigen::Vector3d latLonToEcef(const Eigen::Vector3d & _latlon);
+  Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d & _ecef);
+  Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d & _latlon);
+  Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_ecef);
+  Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_enu);
+
+  void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF);
+  void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF);
 
   double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef);
 
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 948e41edfd0ca7d624dd8a1ec98215b7e9afaa17..50ebf67d4a0e1fd5c24c36acd902508a6206c828 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -61,4 +61,11 @@ INSTALL(TARGETS ${PROJECT_NAME}
 INSTALL(FILES ${HEADERS} DESTINATION include/iri-algorithms/gnss_utils)
 INSTALL(FILES ../Findgnss_utils.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
 
-ADD_SUBDIRECTORY(examples)
+# Testing
+if(BUILD_TESTS)
+  	MESSAGE("Building tests.")
+  	add_subdirectory(test)
+endif()
+
+# Examples
+ADD_SUBDIRECTORY(examples)
\ No newline at end of file
diff --git a/src/examples/gnss_utils_test.cpp b/src/examples/gnss_utils_test.cpp
index ab9c2cf5045ae36a932f1947acfce27b49d2f667..fb0792acec09adc79a13e7ea13c1914c96226018 100644
--- a/src/examples/gnss_utils_test.cpp
+++ b/src/examples/gnss_utils_test.cpp
@@ -88,7 +88,7 @@ int main(int argc, char *argv[])
   std::cout << "msg: " << msg << std::endl;
   std::cout << "sol.stat: " << int(solb.stat) << std::endl;
   std::cout << "Position:      " << solb.rr[0] << ", " << solb.rr[1] << ", " << solb.rr[2] << std::endl;
-  std::cout << "Position (GT): " << latLonToEcef(lla_gt).transpose() << std::endl;
-  std::cout << "Position LLA:      " << ecefToLatLon(Eigen::Vector3d(solb.rr[0],solb.rr[1],solb.rr[2])).transpose() << std::endl;
+  std::cout << "Position (GT): " << latLonAltToEcef(lla_gt).transpose() << std::endl;
+  std::cout << "Position LLA:      " << ecefToLatLonAlt(Eigen::Vector3d(solb.rr[0],solb.rr[1],solb.rr[2])).transpose() << std::endl;
   std::cout << "Position LLA (GT): " << lla_gt.transpose() << std::endl;
 }
diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp
index 5f55ef8254116084ca1c9378e006f64533bf422d..e4336e53f3972c4c57dec41f48c1ffb3889fee34 100644
--- a/src/gnss_utils.cpp
+++ b/src/gnss_utils.cpp
@@ -1,6 +1,5 @@
 #include "gnss_utils/gnss_utils.h"
 
-
 namespace GNSSUtils
 {
   
@@ -48,7 +47,7 @@ namespace GNSSUtils
     output.ns    = sol.ns;
     output.age   = sol.age;
     output.ratio = sol.ratio;
-    output.lat_lon = ecefToLatLon(output.pos);
+    output.lat_lon = ecefToLatLonAlt(output.pos);
 
     return output;
   }
@@ -97,7 +96,7 @@ namespace GNSSUtils
     output.ns    = sol.ns;
     output.age   = sol.age;
     output.ratio = sol.ratio;
-    output.lat_lon = ecefToLatLon(output.pos);
+    output.lat_lon = ecefToLatLonAlt(output.pos);
 
     return output;
     }
@@ -247,7 +246,7 @@ namespace GNSSUtils
       return 0;
   }
 
-Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef)
+Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d & _ecef)
 {
     Eigen::Vector3d pos;
     ecef2pos(_ecef.data(), pos.data());
@@ -255,7 +254,7 @@ Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef)
     return pos;
 }
 
-Eigen::Vector3d latLonToEcef(const Eigen::Vector3d & _pos)
+Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d & _pos)
 {
     Eigen::Vector3d ecef;
     pos2ecef(_pos.data(), ecef.data());
@@ -263,6 +262,131 @@ Eigen::Vector3d latLonToEcef(const Eigen::Vector3d & _pos)
     return ecef;
 }
 
+
+Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_ecef)
+{
+    Eigen::Matrix3d cov_enu;
+
+    /* RTKLIB transform covariance to local tangental coordinate --------------------------
+    * transform ecef covariance to local tangental coordinate
+    * args   : double *pos      I   geodetic position {lat,lon} (rad)
+    *          double *P        I   covariance in ecef coordinate
+    *          double *Q        O   covariance in local tangental coordinate
+    * return : none
+    *-----------------------------------------------------------------------------*/
+    //extern void covenu(const double *pos, const double *P, double *Q);
+    covenu(_latlon.data(), _cov_ecef.data(), cov_enu.data());
+
+    return cov_enu;
+}
+
+Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_enu)
+{
+    Eigen::Matrix3d cov_ecef;
+
+    /* RTKLIB transform local enu coordinate covariance to xyz-ecef -----------------------
+    * transform local enu covariance to xyz-ecef coordinate
+    * args   : double *pos      I   geodetic position {lat,lon} (rad)
+    *          double *Q        I   covariance in local enu coordinate
+    *          double *P        O   covariance in xyz-ecef coordinate
+    * return : none
+    *-----------------------------------------------------------------------------*/
+    //extern void covecef(const double *pos, const double *Q, double *P)
+    covecef(_latlon.data(), _cov_enu.data(), cov_ecef.data());
+
+    return cov_ecef;
+}
+
+void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF)
+{
+  // Convert ECEF coordinates to geodetic coordinates.
+  // J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
+  // to geodetic coordinates," IEEE Transactions on Aerospace and
+  // Electronic Systems, vol. 30, pp. 957-961, 1994.
+
+//  double r = std::sqrt(_t_ECEF_ENU(0) * _t_ECEF_ENU(0) + _t_ECEF_ENU(1) * _t_ECEF_ENU(1));
+//  double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
+//  double F = 54 * kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) * _t_ECEF_ENU(2);
+//  double G = r * r + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) - kFirstEccentricitySquared * Esq;
+//  double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3);
+//  double S = cbrt(1 + C + sqrt(C * C + 2 * C));
+//  double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
+//  double Q = sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P);
+//  double r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q)
+//               + sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q)
+//               - P * (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) / (Q * (1 + Q)) - 0.5 * P * r * r);
+//  double V = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2));
+//  double Z_0 = kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) / (kSemimajorAxis * V);
+//
+//  double latitude = atan((_t_ECEF_ENU(2) + kSecondEccentricitySquared * Z_0) / r);
+//  double longitude = atan2(_t_ECEF_ENU(1), _t_ECEF_ENU(0));
+//
+//
+//  double sLat = sin(latitude);
+//  double cLat = cos(latitude);
+//  double sLon = sin(longitude);
+//  double cLon = cos(longitude);
+//
+//  R_ENU_ECEF(0,0) = -sLon;
+//  R_ENU_ECEF(0,1) =  cLon;
+//  R_ENU_ECEF(0,2) =  0.0;
+//
+//  R_ENU_ECEF(1,0) = -sLat*cLon;
+//  R_ENU_ECEF(1,1) = -sLat * sLon;
+//  R_ENU_ECEF(1,2) =  cLat;
+//
+//  R_ENU_ECEF(2,0) =  cLat * cLon;
+//  R_ENU_ECEF(2,1) =  cLat * sLon;
+//  R_ENU_ECEF(2,2) =  sLat;
+//
+//  t_ENU_ECEF = -R_ENU_ECEF*_t_ECEF_ENU;
+
+    Eigen::Vector3d ENU_lat_lon_alt = GNSSUtils::ecefToLatLonAlt(_t_ECEF_ENU);
+
+    double sLat = sin(ENU_lat_lon_alt(0));
+    double cLat = cos(ENU_lat_lon_alt(0));
+    double sLon = sin(ENU_lat_lon_alt(1));
+    double cLon = cos(ENU_lat_lon_alt(1));
+
+    R_ENU_ECEF(0,0) = -sLon;
+    R_ENU_ECEF(0,1) =  cLon;
+    R_ENU_ECEF(0,2) =  0.0;
+
+    R_ENU_ECEF(1,0) = -sLat*cLon;
+    R_ENU_ECEF(1,1) = -sLat * sLon;
+    R_ENU_ECEF(1,2) =  cLat;
+
+    R_ENU_ECEF(2,0) =  cLat * cLon;
+    R_ENU_ECEF(2,1) =  cLat * sLon;
+    R_ENU_ECEF(2,2) =  sLat;
+
+    t_ENU_ECEF = -R_ENU_ECEF * _t_ECEF_ENU;
+}
+
+void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF)
+{
+    double sLat = sin(_ENU_latlonalt(0));
+    double cLat = cos(_ENU_latlonalt(0));
+    double sLon = sin(_ENU_latlonalt(1));
+    double cLon = cos(_ENU_latlonalt(1));
+
+    R_ENU_ECEF(0,0) = -sLon;
+    R_ENU_ECEF(0,1) =  cLon;
+    R_ENU_ECEF(0,2) =  0.0;
+
+    R_ENU_ECEF(1,0) = -sLat*cLon;
+    R_ENU_ECEF(1,1) = -sLat * sLon;
+    R_ENU_ECEF(1,2) =  cLat;
+
+    R_ENU_ECEF(2,0) =  cLat * cLon;
+    R_ENU_ECEF(2,1) =  cLat * sLon;
+    R_ENU_ECEF(2,2) =  sLat;
+
+    Eigen::Vector3d t_ECEF_ENU = latLonAltToEcef(_ENU_latlonalt);
+
+    t_ENU_ECEF = -R_ENU_ECEF * t_ECEF_ENU;
+}
+
 double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef)
 {
     // ecef 2 geodetic
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..dcf7deb1c88398bfe18e60e9499ebdfdc152fd58
--- /dev/null
+++ b/src/test/CMakeLists.txt
@@ -0,0 +1,17 @@
+# 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        #
+# gnss_utils_add_gtest(gtest_example gtest_example.cpp)      #
+# target_link_libraries(gtest_example ${PROJECT_NAME})       #
+#                                                            #
+##############################################################
+
+# Transformations test
+gnss_utils_add_gtest(gtest_transformations gtest_transformations.cpp)
+target_link_libraries(gtest_transformations ${PROJECT_NAME})
diff --git a/src/test/gtest/CMakeLists.txt b/src/test/gtest/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3c3c48c74516488a8c4eb24c2492bd81a5edc7f9
--- /dev/null
+++ b/src/test/gtest/CMakeLists.txt
@@ -0,0 +1,65 @@
+cmake_minimum_required(VERSION 2.8.8)
+project(gtest_builder C CXX)
+
+# We need thread support
+#find_package(Threads REQUIRED)
+
+# Enable ExternalProject CMake module
+include(ExternalProject)
+
+set(GTEST_FORCE_SHARED_CRT ON)
+set(GTEST_DISABLE_PTHREADS OFF)
+
+# 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()
+
+# 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
+)
+
+# Get GTest source and binary directories from CMake project
+
+# Specify include dir
+ExternalProject_Get_Property(googletest source_dir)
+set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
+
+# Specify MainTest's link libraries
+ExternalProject_Get_Property(googletest binary_dir)
+set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest 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)
+
+# Set libgtest properties
+set_target_properties(libgtest PROPERTIES
+    "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
+    "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
+)
+
+function(gnss_utils_add_gtest target)
+  add_executable(${target} ${ARGN})
+  add_dependencies(${target} libgtest)
+  target_link_libraries(${target} libgtest)
+
+  #WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/bin
+  add_test(NAME ${target} COMMAND ${target})
+endfunction()
diff --git a/src/test/gtest/utils_gtest.h b/src/test/gtest/utils_gtest.h
new file mode 100644
index 0000000000000000000000000000000000000000..09c290758a8b953abc270a386ab4964075ebfdf8
--- /dev/null
+++ b/src/test/gtest/utils_gtest.h
@@ -0,0 +1,146 @@
+/**
+ * \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 GNSSUTILS_UTILS_GTEST_H
+#define GNSSUTILS_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::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
+                  return (lhs - rhs).isMuchSmallerThan(1, precision); \
+               }, \
+               C_expect, C_actual);
+
+#define ASSERT_MATRIX_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd 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::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
+                   MatrixXd er = lhs - rhs; \
+                   er(2) = pi2pi((double)er(2)); \
+                   return er.isMuchSmallerThan(1, precision); \
+               }, \
+               C_expect, C_actual);
+
+#define ASSERT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
+                   MatrixXd er = lhs - rhs; \
+                   er(2) = pi2pi((double)er(2)); \
+                   return er.isMuchSmallerThan(1, precision); \
+               }, \
+               C_expect, C_actual);
+
+} // namespace testing
+
+#endif /* GNSSUTILS_UTILS_GTEST_H */
diff --git a/src/test/gtest_example.cpp b/src/test/gtest_example.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7dd3b9ceea4ff0b3c7ea1c6f8db3286b273f7b66
--- /dev/null
+++ b/src/test/gtest_example.cpp
@@ -0,0 +1,20 @@
+#include "gtest/utils_gtest.h"
+
+TEST(TestTest, DummyTestExample)
+{
+  EXPECT_FALSE(false);
+
+  ASSERT_TRUE(true);
+
+  int my_int = 5;
+
+  ASSERT_EQ(my_int, 5);
+
+//  PRINTF("All good at TestTest::DummyTestExample !\n");
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/src/test/gtest_transformations.cpp b/src/test/gtest_transformations.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..193cb06e478ce3648f956d1ff2491262b411d173
--- /dev/null
+++ b/src/test/gtest_transformations.cpp
@@ -0,0 +1,33 @@
+#include "gtest/utils_gtest.h"
+#include "gnss_utils/gnss_utils.h"
+
+/* functions being tested:
+ *
+ *   Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d & _ecef);
+ *   Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d & _latlon);
+ *   Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_ecef);
+ *   Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_enu);
+ *   void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF);
+ *   void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF);
+ */
+
+using namespace GNSSUtils;
+
+TEST(TransformationsTest, ecefToLatLonAlt)
+{
+    Eigen::Vector3d ecef, latlonalt;
+    ecef << 0, 0, 1e3;
+
+    latlonalt = ecefToLatLonAlt(ecef);
+
+    std::cout << latlonalt.transpose();
+
+    ASSERT_DOUBLE_EQ(latlonalt(0),0.0);
+    ASSERT_DOUBLE_EQ(latlonalt(1),0.0);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}