diff --git a/CMakeLists.txt b/CMakeLists.txt
index 33c532478665f879b3658b0f173f2745d25bc8cc..1403ef88cc5639f5bc06e05882343b4799d3e30d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -49,11 +49,12 @@ SET(RTKLIB_SRC_DIR ${RTKLIB_DIR}/src)
 
 # driver source files
 SET(SOURCES
-    src/gnss_utils.cpp
+    src/utils/gnss_utils.cpp
+    src/utils/utils.cpp
+    src/utils/transformations.cpp
     src/observations.cpp
     src/navigation.cpp
     src/TDCP.cpp
-    src/utils.cpp
     src/ublox_raw.cpp)
 
 SET(RTKLIB_SRC
@@ -88,8 +89,9 @@ SET(RTKLIB_SRC
 
 # application header files
 SET(HEADERS
-    include/gnss_utils/gnss_utils.h
-    include/gnss_utils/utils.h
+    include/gnss_utils/utils/gnss_utils.h
+    include/gnss_utils/utils/utils.h
+    include/gnss_utils/utils/transformations.h
     include/gnss_utils/observations.h
     include/gnss_utils/navigation.h
     include/gnss_utils/TDCP.h
diff --git a/include/gnss_utils/TDCP.h b/include/gnss_utils/TDCP.h
index 439a981ad097ea568d1cda9321b31c9e1b86fa55..130d9ebff6ee81e39776ec71608aa038bc540cfe 100644
--- a/include/gnss_utils/TDCP.h
+++ b/include/gnss_utils/TDCP.h
@@ -11,7 +11,7 @@
 #define GNSS_UTILS_TDCP_DEBUG 0
 
 #include <set>
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/gnss_utils.h"
 #include "gnss_utils/observations.h"
 #include "gnss_utils/navigation.h"
 
diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h
index 5610a7ec08698c19a6cf84152da0773b83f55cb2..9b0cb1ddea91f7cccb51a8a0339fa42b2313d731 100644
--- a/include/gnss_utils/observations.h
+++ b/include/gnss_utils/observations.h
@@ -7,7 +7,7 @@
 #include <memory>
 #include <cassert>
 
-#include "gnss_utils/utils.h"
+#include "gnss_utils/utils/utils.h"
 
 extern "C" {
 #include "rtklib.h"
diff --git a/include/gnss_utils/ublox_raw.h b/include/gnss_utils/ublox_raw.h
index c3e4cc15180559331f3b0732f4243eb684076b63..7c8c6eb25fb2a23ed643575b5a17ddc96e5dafa8 100644
--- a/include/gnss_utils/ublox_raw.h
+++ b/include/gnss_utils/ublox_raw.h
@@ -1,7 +1,7 @@
 #ifndef UBLOX_RAW_H
 #define UBLOX_RAW_H
 
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/gnss_utils.h"
 
 namespace GNSSUtils
 {
diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/utils/gnss_utils.h
similarity index 80%
rename from include/gnss_utils/gnss_utils.h
rename to include/gnss_utils/utils/gnss_utils.h
index a16976acdec9a321d0b47fa1f4c647fab896940b..af36b624015fb3c3c24e15d585754f82263bd747 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/utils/gnss_utils.h
@@ -12,6 +12,7 @@
 
 #include "gnss_utils/observations.h"
 #include "gnss_utils/navigation.h"
+#include "gnss_utils/utils/transformations.h"
 
 extern "C" {
 #include "rtklib.h"
@@ -62,18 +63,6 @@ int estposOwn(const obsd_t*   obs,
               double*         resp,
               char*           msg);
 
-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);
 
 void computeSatellitesPositions(const Observations&             obs,
diff --git a/include/gnss_utils/utils/transformations.h b/include/gnss_utils/utils/transformations.h
new file mode 100644
index 0000000000000000000000000000000000000000..a13f6ba6974069bdcf55fb6b260e02afbe24ae3e
--- /dev/null
+++ b/include/gnss_utils/utils/transformations.h
@@ -0,0 +1,39 @@
+/*
+ * TDCP.h
+ *
+ *  Created on: April 3, 2020
+ *      Author: Pep Martí-Saumell
+ */
+
+#ifndef INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
+#define INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
+
+// #include <vector>
+// #include <iostream>
+// #include <memory>
+// #include <string>
+
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+#include <eigen3/Eigen/Sparse>
+
+extern "C" {
+#include "rtklib.h"
+}
+
+namespace GNSSUtils
+{
+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);
+
+}  // namespace GNSSUtils
+#endif  // INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
\ No newline at end of file
diff --git a/include/gnss_utils/utils.h b/include/gnss_utils/utils/utils.h
similarity index 100%
rename from include/gnss_utils/utils.h
rename to include/gnss_utils/utils/utils.h
diff --git a/src/examples/gnss_utils_test.cpp b/src/examples/gnss_utils_test.cpp
index 814ee443c905e3a1fd8d327bcad4d096227a02f6..df97ca9f274202d7b7b107e5ff8721c8ca120add 100644
--- a/src/examples/gnss_utils_test.cpp
+++ b/src/examples/gnss_utils_test.cpp
@@ -1,6 +1,6 @@
 #include "gnss_utils/observations.h"
 #include "gnss_utils/navigation.h"
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/gnss_utils.h"
 
 #include <typeinfo>
 
diff --git a/src/observations.cpp b/src/observations.cpp
index ea2dc15d658ce4644e8446ae71182ce21c428d4a..3e8762dde80040272512a65feeba3c3394cbc410 100644
--- a/src/observations.cpp
+++ b/src/observations.cpp
@@ -1,4 +1,4 @@
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/gnss_utils.h"
 #include "gnss_utils/observations.h"
 
 using namespace GNSSUtils;
diff --git a/src/gnss_utils.cpp b/src/utils/gnss_utils.cpp
similarity index 63%
rename from src/gnss_utils.cpp
rename to src/utils/gnss_utils.cpp
index 70bad327efe878de41efd6fd2c21d5f7f4e51528..feca02da165f8470cd0219b6b754d978cef7fb5a 100644
--- a/src/gnss_utils.cpp
+++ b/src/utils/gnss_utils.cpp
@@ -1,4 +1,4 @@
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/gnss_utils.h"
 
 namespace GNSSUtils
 {
@@ -19,38 +19,35 @@ ComputePosOutput computePos(const GNSSUtils::Observations& _observations,
   sol_t                       sol;
   sol = { { 0 } };
 
-    output.pos_stat = pntpos(_observations.data(), _observations.size(),
-                            &(_navigation.getNavigation()),
-                            &_prcopt, &sol, NULL, NULL, msg);
-    
-    if (output.pos_stat == 0)
-    {
-      std::cout << "computePos: error in computing positioning, message: "  << msg << "\n";
-    }
-
-    output.time = sol.time.time;
-    output.sec = sol.time.sec;
-    output.pos  = Eigen::Vector3d(sol.rr);
-    output.vel  = Eigen::Vector3d(&sol.rr[3]);
-    output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5],
-                        sol.qr[3], sol.qr[1], sol.qr[4],
-                        sol.qr[5], sol.qr[4], sol.qr[2];
-    //std::cout << "Compute pos:  " << output.pos.transpose() << "\n";
-    //std::cout << "Covariance:\n" << output.pos_covar << "\n";
-
-    // XXX: segmentation fault here.
-    // if (sol.dtr != NULL)
-    // {
-    //   output.rcv_bias << sol.dtr[0], sol.dtr[1], sol.dtr[2], sol.dtr[3], sol.dtr[4], sol.dtr[5];
-    // }
-    output.type  = sol.type;
-    output.stat  = sol.stat;
-    output.ns    = sol.ns;
-    output.age   = sol.age;
-    output.ratio = sol.ratio;
-    output.lat_lon = ecefToLatLonAlt(output.pos);
-
-    return output;
+  output.pos_stat = pntpos(
+      _observations.data(), _observations.size(), &(_navigation.getNavigation()), &_prcopt, &sol, NULL, NULL, msg);
+
+  if (output.pos_stat == 0)
+  {
+    std::cout << "computePos: error in computing positioning, message: " << msg << "\n";
+  }
+
+  output.time = sol.time.time;
+  output.sec  = sol.time.sec;
+  output.pos  = Eigen::Vector3d(sol.rr);
+  output.vel  = Eigen::Vector3d(&sol.rr[3]);
+  output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5], sol.qr[3], sol.qr[1], sol.qr[4], sol.qr[5], sol.qr[4], sol.qr[2];
+  // std::cout << "Compute pos:  " << output.pos.transpose() << "\n";
+  // std::cout << "Covariance:\n" << output.pos_covar << "\n";
+
+  // XXX: segmentation fault here.
+  // if (sol.dtr != NULL)
+  // {
+  //   output.rcv_bias << sol.dtr[0], sol.dtr[1], sol.dtr[2], sol.dtr[3], sol.dtr[4], sol.dtr[5];
+  // }
+  output.type    = sol.type;
+  output.stat    = sol.stat;
+  output.ns      = sol.ns;
+  output.age     = sol.age;
+  output.ratio   = sol.ratio;
+  output.lat_lon = ecefToLatLonAlt(output.pos);
+
+  return output;
 }
 
 // ComputePosOutput computePosOwn(const GNSSUtils::Observations & _observations,
@@ -256,150 +253,6 @@ int estposOwn(const obsd_t*   obs,
   return 0;
 }
 
-Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef)
-{
-  Eigen::Vector3d pos;
-  ecef2pos(_ecef.data(), pos.data());
-
-  return pos;
-}
-
-Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _pos)
-{
-  Eigen::Vector3d ecef;
-  pos2ecef(_pos.data(), ecef.data());
-
-  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/utils/transformations.cpp b/src/utils/transformations.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fa8c0e247fa7d1107afe0f17501ee9d8324b3341
--- /dev/null
+++ b/src/utils/transformations.cpp
@@ -0,0 +1,147 @@
+#include "gnss_utils/utils/transformations.h"
+
+using namespace GNSSUtils;
+
+Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef)
+{
+  Eigen::Vector3d pos;
+  ecef2pos(_ecef.data(), pos.data());
+
+  return pos;
+}
+
+Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _pos)
+{
+  Eigen::Vector3d ecef;
+  pos2ecef(_pos.data(), ecef.data());
+
+  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;
+}
\ No newline at end of file
diff --git a/src/utils.cpp b/src/utils/utils.cpp
similarity index 97%
rename from src/utils.cpp
rename to src/utils/utils.cpp
index cd697b39951aa113fb7a160f9b75722cbdf740f7..3dcd1e2731871f74fd04d26ee1d1a26a788b5d17 100644
--- a/src/utils.cpp
+++ b/src/utils/utils.cpp
@@ -1,4 +1,4 @@
-#include "gnss_utils/utils.h"
+#include "gnss_utils/utils/utils.h"
 
 namespace GNSSUtils
 {
diff --git a/test/gtest_navigation.cpp b/test/gtest_navigation.cpp
index 5170cf11310248899fd967b3b870f974a6368b2f..18e8c638aa4ce62dcdfaa0a702f7be18652c5c9c 100644
--- a/test/gtest_navigation.cpp
+++ b/test/gtest_navigation.cpp
@@ -1,5 +1,5 @@
 #include "gtest/utils_gtest.h"
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/gnss_utils.h"
 
 TEST(NavigationTests, Whatever)
 {
diff --git a/test/gtest_observations.cpp b/test/gtest_observations.cpp
index fc24115a0c7198f24b9b19718d219a9928ef78db..57244d40ac7990173159b6fd387c16ee4be022b2 100644
--- a/test/gtest_observations.cpp
+++ b/test/gtest_observations.cpp
@@ -1,5 +1,5 @@
 #include "gtest/utils_gtest.h"
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/gnss_utils.h"
 #include "gnss_utils/observations.h"
 
 using namespace GNSSUtils;
diff --git a/test/gtest_transformations.cpp b/test/gtest_transformations.cpp
index cae4e24bb4bd45e99fceaa07a201fe36926b5f78..8b819bb63bf4ce6174e2dfc7ce5f5035f7202c1b 100644
--- a/test/gtest_transformations.cpp
+++ b/test/gtest_transformations.cpp
@@ -1,5 +1,5 @@
 #include "gtest/utils_gtest.h"
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/gnss_utils.h"
 
 // Geodetic system parameters
 static double kSemimajorAxis = 6378137;