diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index 9d68ae9d0001e648ed8448324f34776075c7d845..a550db34940dda8eb3bee9c2a39cbab4cd1e6ab5 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/gnss_utils.h
@@ -4,6 +4,11 @@
 #include <vector>
 #include <iostream>
 #include <memory>
+
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+#include <eigen3/Eigen/Sparse>
+
 #include "observations.h"
 #include "navigation.h"
 
@@ -17,5 +22,7 @@ namespace GNSSUtils
   int getPos(const std::shared_ptr<Observations> & _observations,
             const std::shared_ptr<Navigation> & _navigation,
             sol_t & _sol);
+
+  Eigen::Vector3d ecefToPos(const Eigen::Vector3d & _ecef);
 }
 #endif
diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp
index 0eb5ab16d2b35bd1e189088c77c4e8557b4a27b6..b5c9c0a4f85ce78618e041eb07d88fa1186b1791 100644
--- a/src/gnss_utils.cpp
+++ b/src/gnss_utils.cpp
@@ -36,4 +36,12 @@ namespace GNSSUtils
 
     return stat;
   }
+
+  Eigen::Vector3d ecefToPos(const Eigen::Vector3d & _ecef)
+  {
+    double pos[3];
+    ecef2pos(&_ecef(0), pos);
+
+    return Eigen::Map<Eigen::Vector3d>(pos);
+  }
 }