Added function ecefToPos()

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

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);
+  }
 }