diff --git a/CMakeLists.txt b/CMakeLists.txt
index b0da1a80eb594eb158b4901a1ceb7f0c026fabc9..33c532478665f879b3658b0f173f2745d25bc8cc 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -39,7 +39,106 @@ endif()
 
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fpermissive")
 
-ADD_SUBDIRECTORY(src)
+IF(NOT BUILD_TESTS)
+  OPTION(BUILD_TESTS "Build Unit tests" ON)
+ENDIF(NOT BUILD_TESTS)
+
+# rtklib path
+SET(RTKLIB_DIR deps/RTKLIB)
+SET(RTKLIB_SRC_DIR ${RTKLIB_DIR}/src)
+
+# driver source files
+SET(SOURCES
+    src/gnss_utils.cpp
+    src/observations.cpp
+    src/navigation.cpp
+    src/TDCP.cpp
+    src/utils.cpp
+    src/ublox_raw.cpp)
+
+SET(RTKLIB_SRC
+    ${RTKLIB_SRC_DIR}/pntpos.c
+    ${RTKLIB_SRC_DIR}/rtkcmn.c
+    ${RTKLIB_SRC_DIR}/sbas.c
+    ${RTKLIB_SRC_DIR}/ephemeris.c
+    ${RTKLIB_SRC_DIR}/preceph.c
+    ${RTKLIB_SRC_DIR}/qzslex.c
+    ${RTKLIB_SRC_DIR}/rtcm.c
+    ${RTKLIB_SRC_DIR}/rtcm2.c
+    ${RTKLIB_SRC_DIR}/rtcm3.c
+    ${RTKLIB_SRC_DIR}/rtcm3e.c
+    ${RTKLIB_SRC_DIR}/ionex.c
+    ${RTKLIB_SRC_DIR}/rinex.c
+    ${RTKLIB_SRC_DIR}/rcvraw.c
+    ${RTKLIB_SRC_DIR}/rcv/binex.c
+    ${RTKLIB_SRC_DIR}/rcv/cmr.c
+    ${RTKLIB_SRC_DIR}/rcv/comnav.c
+    ${RTKLIB_SRC_DIR}/rcv/crescent.c
+    ${RTKLIB_SRC_DIR}/rcv/gw10.c
+    ${RTKLIB_SRC_DIR}/rcv/javad.c
+    ${RTKLIB_SRC_DIR}/rcv/novatel.c
+    ${RTKLIB_SRC_DIR}/rcv/nvs.c
+    ${RTKLIB_SRC_DIR}/rcv/rt17.c
+    ${RTKLIB_SRC_DIR}/rcv/septentrio.c
+    ${RTKLIB_SRC_DIR}/rcv/skytraq.c
+    ${RTKLIB_SRC_DIR}/rcv/swiftnav.c
+    ${RTKLIB_SRC_DIR}/rcv/tersus.c
+    ${RTKLIB_SRC_DIR}/rcv/ublox.c)
+
+
+# application header files
+SET(HEADERS
+    include/gnss_utils/gnss_utils.h
+    include/gnss_utils/utils.h
+    include/gnss_utils/observations.h
+    include/gnss_utils/navigation.h
+    include/gnss_utils/TDCP.h
+    include/gnss_utils/ublox_raw.h
+  ${RTKLIB_SRC_DIR}/rtklib.h)
+
+# Eigen #######
+FIND_PACKAGE(Eigen3 REQUIRED)
+
+# Boost #######
+set(Boost_USE_STATIC_LIBS OFF)
+set(Boost_USE_MULTITHREADED OFF)
+set(Boost_USE_STATIC_RUNTIME OFF)
+FIND_PACKAGE(Boost REQUIRED)
+
+# Adding libraries' directories
+link_directories(/usr/lib/x86_64-linux-gnu/)
+
+# Adding include directories
+INCLUDE_DIRECTORIES(include/ ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${RTKLIB_SRC_DIR})
+
+# create the shared library
+ADD_LIBRARY(gnss_utils SHARED ${SOURCES} ${RTKLIB_SRC})
+TARGET_LINK_LIBRARIES(gnss_utils ${Boost_LIBRARIES})
+
+# Installing
+INSTALL(TARGETS ${PROJECT_NAME}
+      RUNTIME DESTINATION bin
+      LIBRARY DESTINATION lib/iri-algorithms
+      ARCHIVE DESTINATION lib/iri-algorithms)
+INSTALL(FILES ${HEADERS} DESTINATION include/iri-algorithms/gnss_utils)
+INSTALL(FILES Findgnss_utils.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
+
+
+# Examples
+ADD_SUBDIRECTORY(src/examples)
+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
+  	MESSAGE("Building tests.")
+    enable_testing()
+endif()
+
+# Testing
+if(BUILD_TESTS)
+  	add_subdirectory(test)
+endif()
 
 FIND_PACKAGE(Doxygen)
 
diff --git a/README.md b/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..8db7c470a0e95d47e2f01e033e8636ba77959a91
--- /dev/null
+++ b/README.md
@@ -0,0 +1,26 @@
+# Installation
+
+Clone the repository in your chosen directory. In a new terminal:
+
+```
+cd <your/path>
+git clone <repo-link>
+```
+
+Clone the RTKLIB lib as a sub-module:
+```
+git submodule update --init
+```
+
+Create a directory to build all the source files:
+```
+$ mkdir build
+$ cd build
+```
+
+Compile the source files:
+
+```
+$ cmake ..
+$ make
+```
\ No newline at end of file
diff --git a/include/gnss_utils/TDCP.h b/include/gnss_utils/TDCP.h
new file mode 100644
index 0000000000000000000000000000000000000000..439a981ad097ea568d1cda9321b31c9e1b86fa55
--- /dev/null
+++ b/include/gnss_utils/TDCP.h
@@ -0,0 +1,85 @@
+/*
+ * TDCP.h
+ *
+ *  Created on: Dec 4, 2019
+ *      Author: jvallve
+ */
+
+#ifndef INCLUDE_GNSS_UTILS_TDCP_H_
+#define INCLUDE_GNSS_UTILS_TDCP_H_
+
+#define GNSS_UTILS_TDCP_DEBUG 0
+
+#include <set>
+#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
+
+namespace GNSSUtils
+{
+struct TDCPParams
+{
+  int    min_common_sats;
+  int    raim_n;
+  double raim_min_residual;
+  bool   use_carrier_phase;
+  bool   correct_tropo;
+  bool   correct_iono;
+  bool   relinearize_jacobian;
+  int    max_iterations;
+  double sigma_atm;
+  double sigma_code;
+  double sigma_carrier;
+  bool   use_old_nav;
+  bool   use_multi_freq;
+  double time_window;
+};
+
+bool TDCP(const Observations& obs_r,
+          Navigation&         nav_r,
+          const Observations& obs_k,
+          const Navigation&   nav_k,
+          Eigen::Vector4d&    d,
+          Eigen::Matrix4d&    cov_d,
+          double&             residual,
+          std::set<int>&      discarded_sats,
+          const TDCPParams&   sd_opt,
+          const prcopt_t&     opt);
+
+bool TDCP(const Observations&    obs_r,
+          const Navigation&      nav_r,
+          const Eigen::Vector3d& x_r,
+          const Observations&    obs_k,
+          const Navigation&      nav_k,
+          Eigen::Vector4d&       d,
+          Eigen::Matrix4d&       cov_d,
+          double&                residual,
+          std::set<int>&         discarded_sats,
+          const TDCPParams&      sd_opt,
+          const prcopt_t&        opt);
+
+bool TDCP(const Observations&                   common_obs_r,
+          const Navigation&                     nav_r,
+          const std::map<int, Eigen::Vector3d>& common_sats_pos_r,
+          const Eigen::Vector3d&                x_r,
+          const Observations&                   common_obs_k,
+          const Navigation&                     nav_k,
+          const std::map<int, Eigen::Vector3d>& common_sats_pos_k,
+          Eigen::Vector4d&                      d,
+          Eigen::Matrix4d&                      cov_d,
+          double&                               residual,
+          std::set<int>&                        discarded_sats,
+          const TDCPParams&                     sd_opt);
+
+void filterCommonSatellites(Observations&                   common_obs_r,
+                            std::map<int, Eigen::Vector3d>& common_sats_pos_r,
+                            Observations&                   common_obs_k,
+                            std::map<int, Eigen::Vector3d>& common_sats_pos_k,
+                            std::set<int>&                  discarded_sats,
+                            const Eigen::Vector3d&          x_r,
+                            const TDCPParams&               sd_params,
+                            const prcopt_t&                 opt);
+
+}  // namespace GNSSUtils
+
+#endif /* INCLUDE_GNSS_UTILS_TDCP_H_ */
diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index 626030968e0fa13278085403435d955227a8a2f3..a16976acdec9a321d0b47fa1f4c647fab896940b 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/gnss_utils.h
@@ -10,39 +10,95 @@
 #include <eigen3/Eigen/Geometry>
 #include <eigen3/Eigen/Sparse>
 
-#include "observations.h"
-#include "navigation.h"
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
 
-extern "C"
-{
-    #include "rtklib.h"
+extern "C" {
+#include "rtklib.h"
 }
 
 namespace GNSSUtils
 {
-  struct ComputePosOutput{
-    time_t time;
-    double sec;
-    Eigen::Vector3d pos;        // position (m)
-    Eigen::Vector3d vel;        // velocity (m/s)
-    Eigen::Matrix3d pos_covar;  // position covariance (m^2)
-                                // {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx}
-    Eigen::VectorXd rcv_bias;   // receiver clock bias to time systems (s)
-    int type;                   // type (0:xyz-ecef,1:enu-baseline)
-    int stat;                   // solution status (SOLQ_???)
-    int ns;                     // number of valid satellites
-    double age;                 // age of differential (s)
-    double ratio;               // AR ratio factor for valiation
-
-    int pos_stat;               // return from pntpos
-    Eigen::Vector3d lat_lon;     // latitude_longitude_altitude
-  };
-
-  ComputePosOutput computePos(const Observations & _observations,
-                                    Navigation & _navigation,
-                                    const prcopt_t & _prcopt);
-  
-  Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef);
+struct ComputePosOutput
+{
+  time_t          time;
+  double          sec;
+  Eigen::Vector3d pos;        // position (m)
+  Eigen::Vector3d vel;        // velocity (m/s)
+  Eigen::Matrix3d pos_covar;  // position covariance (m^2)
+                              // {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx}
+  Eigen::VectorXd rcv_bias;   // receiver clock bias to time systems (s)
+  int             type;       // coordinates used (0:xyz-ecef,1:enu-baseline)
+  int             stat;       // solution status (SOLQ_???)
+  int             ns;         // number of valid satellites
+  double          age;        // age of differential (s)
+  double          ratio;      // AR ratio factor for valiation
+
+  int             pos_stat;  // return from pntpos
+  Eigen::Vector3d lat_lon;   // latitude_longitude_altitude
+};
+
+ComputePosOutput computePos(const Observations& _observations, Navigation& _navigation, const prcopt_t& _prcopt);
+
+// ComputePosOutput computePosOwn(const Observations & _observations,
+//                                Navigation & _navigation,
+//                                const prcopt_t & _prcopt);
+
+// int pntposOwn(const obsd_t *obs, int n, const nav_t *nav,
+//               const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
+//               char *msg);
+
+int estposOwn(const obsd_t*   obs,
+              int             n,
+              const double*   rs,
+              const double*   dts,
+              const double*   vare,
+              const int*      svh,
+              const nav_t*    nav,
+              const prcopt_t* opt,
+              sol_t*          sol,
+              double*         azel,
+              int*            vsat,
+              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,
+                                const Navigation&               nav,
+                                const prcopt_t&                 opt,
+                                std::map<int, Eigen::Vector3d>& sats_pos);
+
+template <typename T>
+bool equalArray(const T* array1, const T* array2, const int& size1, const int& size2)
+{
+  if (size1 != size2)
+    return false;
+
+  for (int ii = 0; ii < size1; ++ii)
+  {
+    if (array1[ii] != array2[ii])
+      return false;
+  }
+
+  return true;
 }
+bool equalTime(const gtime_t& time1, const gtime_t& time2);
+bool equalObservations(const obsd_t& obs1, const obsd_t& obs2);
+bool equalObservations(const obs_t& obs1, const obs_t& obs2);
+
+}  // namespace GNSSUtils
 
 #endif
diff --git a/include/gnss_utils/navigation.h b/include/gnss_utils/navigation.h
index d93ed0fe4c1715c3ee11bada887674e452dade2f..20f65a0ffb1e111a5ba68f5a1a4f22693e73b426 100644
--- a/include/gnss_utils/navigation.h
+++ b/include/gnss_utils/navigation.h
@@ -5,58 +5,361 @@
 #include <iostream>
 #include <memory>
 
+extern "C" {
+#include "rtklib.h"
+}
+
+namespace GNSSUtils
+{
+class Navigation;
+typedef std::shared_ptr<Navigation>       NavigationPtr;
+typedef std::shared_ptr<const Navigation> NavigationConstPtr;
 
-extern "C"
+class Navigation
 {
-    #include "rtklib.h"
+public:
+  // Constructor & Destructor
+  Navigation();
+  Navigation(const Navigation& nav);
+  ~Navigation();
+
+  // Public objects
+
+  // Public methods
+  void setNavigation(nav_t nav);
+  void clearNavigation();
+  void
+  loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt = 0.0, const char* opt = "");
+
+  const nav_t& getNavigation() const;
+  nav_t&       getNavigation();
+
+  void uniqueNavigation();  // remove duplicated ephemerides and update carrier phase wave lengths
+
+  /****************** Array memory management ******************/
+  bool addEphemeris(const eph_t& eph);
+  bool addGlonassEphemeris(const geph_t& geph);
+  bool addSbasEphemeris(const seph_t& seph);
+  bool addPreciseEphemeris(const peph_t& peph);
+  bool addPreciseClock(const pclk_t& pclk);
+  bool addAlmanac(const alm_t& alm);
+  bool addTec(const tec_t& tec);
+  bool addFcb(const fcbd_t& fcb);
+
+  void addSbasMessage(const sbsmsg_t& sbas_msg);
+
+  void copyAllArrays(const nav_t& nav);
+  void copyEphemeris(const nav_t& nav);
+  void copyAlmanac(const nav_t& nav);
+  void copyIonUtc(const nav_t& nav);
+  void copySbasCorrections(const nav_t& nav);
+
+  void freeNavigationArrays();
+  void freeEphemeris();
+  void freeGlonassEphemeris();
+  void freeSbasEphemeris();
+  void freePreciseEphemeris();
+  void freeAlmanac();
+  void freePreciseClock();
+  void freeTecData();
+  void freeFcbData();
+
+  void print();
+
+  //////////////////////////////// nav UTILS //////////////////////////////////////
+  template <typename T>
+  static bool addToArray(const T& new_element, T*& array, int& n, int& nmax);
+  template <typename T>
+  static bool copyArray(const T* array_in, const int& n_in, T*& array_out, int& n_out, int& nmax_out);
+  template <typename T>
+  static void freeArray(T*& array, int& n, int& nmax);
+  static void freeEph(nav_t& nav);
+  static void freeGeph(nav_t& nav);
+  static void freeSeph(nav_t& nav);
+  static void freePeph(nav_t& nav);
+  static void freeAlm(nav_t& nav);
+  static void freePclk(nav_t& nav);
+  static void freeTec(nav_t& nav);
+  static void freeFcb(nav_t& nav);
+  static void freeNavArrays(nav_t& nav);
+
+private:
+  // rtklib-like attribute to represent the different navigation msgs for a given epoch
+  nav_t nav_;
+
+  // Private methods
+};
+
+inline void Navigation::uniqueNavigation()  // remove duplicated ephemerides and update wave lengths
+{
+  uniqnav(&nav_);
 }
 
-namespace GNSSUtils
+inline const nav_t& Navigation::getNavigation() const
 {
-  class Navigation
-  {
-    public:
-      // Constructor & Destructor
-      Navigation();
-      ~Navigation();
+  return nav_;
+}
+
+inline nav_t& Navigation::getNavigation()
+{
+  return nav_;
+}
+
+inline bool Navigation::addEphemeris(const eph_t& eph)
+{
+  return addToArray<eph_t>(eph, nav_.eph, nav_.n, nav_.nmax);
+  //    // "inspired" from RTKLIB rinex.c
+  //    eph_t *nav_eph;
+  //
+  //    if (nav_.nmax<=nav_.n) {
+  //        nav_.nmax+=1024;
+  //        if (!(nav_eph=(eph_t *)realloc(nav_.eph,sizeof(eph_t)*nav_.nmax))) {
+  //            printf("addEphemeris malloc error: n=%d\n",nav_.nmax);
+  //            free(nav_.eph); nav_.eph=NULL; nav_.n=nav_.nmax=0;
+  //            return false;
+  //        }
+  //        nav_.eph=nav_eph;
+  //    }
+  //    nav_.eph[nav_.n++]=eph;
+  //    return true;
+}
+
+inline bool Navigation::addGlonassEphemeris(const geph_t& geph)
+{
+  return addToArray<geph_t>(geph, nav_.geph, nav_.ng, nav_.ngmax);
+  //    // "inspired" from RTKLIB rinex.c
+  //    geph_t *nav_geph;
+  //
+  //    if (nav_.ngmax<=nav_.ng) {
+  //        nav_.ngmax+=1024;
+  //        if (!(nav_geph=(geph_t *)realloc(nav_.geph,sizeof(geph_t)*nav_.ngmax))) {
+  //            printf("addGLONASSEphemeris malloc error: n=%d\n",nav_.ngmax);
+  //            free(nav_.geph); nav_.geph=NULL; nav_.ng=nav_.ngmax=0;
+  //            return false;
+  //        }
+  //        nav_.geph=nav_geph;
+  //    }
+  //    nav_.geph[nav_.ng++]=geph;
+  //    return true;
+}
+
+inline bool Navigation::addSbasEphemeris(const seph_t& seph)
+{
+  return addToArray<seph_t>(seph, nav_.seph, nav_.ns, nav_.nsmax);
+  //    // "inspired" from RTKLIB rinex.c
+  //    seph_t *nav_seph;
+  //
+  //    if (nav_.nsmax<=nav_.ns) {
+  //        nav_.nsmax+=1024;
+  //        if (!(nav_seph=(seph_t *)realloc(nav_.seph,sizeof(seph_t)*nav_.nsmax))) {
+  //            printf("addSBASEphemeris malloc error: n=%d\n",nav_.nsmax);
+  //            free(nav_.seph); nav_.seph=NULL; nav_.ns=nav_.nsmax=0;
+  //            return false;
+  //        }
+  //        nav_.seph=nav_seph;
+  //    }
+  //    nav_.seph[nav_.ns++]=seph;
+  //    return true;
+}
+
+inline bool Navigation::addPreciseEphemeris(const peph_t& peph)
+{
+  return addToArray<peph_t>(peph, nav_.peph, nav_.ne, nav_.nemax);
+}
+
+inline bool Navigation::addPreciseClock(const pclk_t& pclk)
+{
+  return addToArray<pclk_t>(pclk, nav_.pclk, nav_.nc, nav_.ncmax);
+}
+
+inline bool Navigation::addAlmanac(const alm_t& alm)
+{
+  return addToArray<alm_t>(alm, nav_.alm, nav_.na, nav_.namax);
+  //    // "inspired" from RTKLIB rinex.c
+  //    alm_t *nav_alm;
+  //
+  //    if (nav_.namax<=nav_.na) {
+  //        nav_.namax+=1024;
+  //        if (!(nav_alm=(alm_t *)realloc(nav_.alm,sizeof(alm_t)*nav_.namax))) {
+  //            printf("addAlmanac malloc error: n=%d\n",nav_.namax);
+  //            free(nav_.alm); nav_.alm=NULL; nav_.na=nav_.namax=0;
+  //            return false;
+  //        }
+  //        nav_.alm=nav_alm;
+  //    }
+  //    nav_.alm[nav_.na++]=alm;
+  //    return true;
+}
+
+inline bool Navigation::addTec(const tec_t& tec)
+{
+  return addToArray<tec_t>(tec, nav_.tec, nav_.nt, nav_.ntmax);
+}
+
+inline bool Navigation::addFcb(const fcbd_t& fcb)
+{
+  return addToArray<fcbd_t>(fcb, nav_.fcb, nav_.nf, nav_.nfmax);
+}
+
+inline void Navigation::addSbasMessage(const sbsmsg_t& sbas_msg)
+{
+  sbsupdatecorr(&sbas_msg, &nav_);
+}
 
-      // Public objects
+inline void Navigation::freeEphemeris()
+{
+  freeEph(nav_);
+}
 
-      // Public methods
+inline void Navigation::freeGlonassEphemeris()
+{
+  freeGeph(nav_);
+}
+
+inline void Navigation::freePreciseEphemeris()
+{
+  freePeph(nav_);
+}
+
+inline void Navigation::freeSbasEphemeris()
+{
+  freeSeph(nav_);
+}
+
+inline void Navigation::freePreciseClock()
+{
+  freePclk(nav_);
+}
+
+inline void Navigation::freeTecData()
+{
+  freeTec(nav_);
+}
+
+inline void Navigation::freeFcbData()
+{
+  freeFcb(nav_);
+}
+
+inline void Navigation::freeAlmanac()
+{
+  freeAlm(nav_);
+}
+
+//////////////////////////////// nav UTILS //////////////////////////////////////
+template <typename T>
+inline bool Navigation::addToArray(const T& new_element, T*& array, int& n, int& nmax)
+{
+  // std::cout << "addToArray: n = " << n << " nmax = " << nmax << "\n";
+  // "inspired" from RTKLIB rinex.c
+  T* array_ref;
+  if (nmax <= n)
+  {
+    // std::cout << "addToArray: nmax <= n\n";
+    nmax += 1024;
+    if (!(array_ref = (T*)realloc(array, sizeof(T) * nmax)))
+    {
+      printf("addToArray malloc error: n=%d\n", nmax);
+      free(array);
+      array = NULL;
+      n = nmax = 0;
+      return false;
+    }
+    // std::cout << "addToArray: assigning reallocated array\n";
+    array = array_ref;
+  }
+  // std::cout << "addToArray: adding element " << n << "\n";
+  array[n++] = new_element;
+  // std::cout << "addToArray: added!\n";
+  return true;
+}
 
-      void clearNavigation();
+template <typename T>
+inline bool Navigation::copyArray(const T* array_in, const int& n_in, T*& array_out, int& n_out, int& nmax_out)
+{
+  // std::cout << "copyArray: " << n_in << " elements\n";
+  if (array_in == NULL)
+    return false;
 
-      void setNavigation(nav_t nav);
+  // std::cout << "copyArray: array in not null\n";
 
-      const nav_t & getNavigation() const;
-      nav_t & getNavigation();
+  for (int i = 0; i < n_in; i++)
+  {
+    // std::cout << "copyArray: adding element " << i << "\n";
+    if (!addToArray<T>(array_in[i], array_out, n_out, nmax_out))
+    {
+      // std::cout << "copyArray: failed to add..\n";
+      return false;
+    }
+    // std::cout << "copyArray: n_out = " << n_out << " nmax_out = " << nmax_out << "\n";
+  }
 
+  // std::cout << "copyArray: all copied\n";
 
-      /****************** Array memory management ******************/
+  return true;
+}
 
+template <typename T>
+void Navigation::freeArray(T*& array, int& n, int& nmax)
+{
+  if (array != NULL)
+    free(array);
+  array = NULL;
+  n = nmax = 0;
+}
 
-      // Ephemeris
-      void allocateEphemeris(int n_sat = MAXSAT);
-      void deleteEphemeris();
+inline void Navigation::freeEph(nav_t& nav)
+{
+  freeArray<eph_t>(nav.eph, nav.n, nav.nmax);
+}
 
-      void allocateGLONASSEphemeris(int n_sat = NSATGLO);
-      void deleteGLONASSEphemeris();
+inline void Navigation::freeGeph(nav_t& nav)
+{
+  freeArray<geph_t>(nav.geph, nav.ng, nav.ngmax);
+}
 
-      void allocateSBASEphemeris(int n_sat = NSATSBS*2); //SBAS
-      void deleteSBASEphemeris();
+inline void Navigation::freeSeph(nav_t& nav)
+{
+  freeArray<seph_t>(nav.seph, nav.ns, nav.nsmax);
+}
 
-      void allocateAlmanac(int n_sat = MAXSAT);
-      void deleteAlmanac();
-      void print();
+inline void Navigation::freePeph(nav_t& nav)
+{
+  freeArray<peph_t>(nav.peph, nav.ne, nav.nemax);
+}
 
-    private:
+inline void Navigation::freeAlm(nav_t& nav)
+{
+  freeArray<alm_t>(nav.alm, nav.na, nav.namax);
+}
 
-      // rtklib-like attribute to represent the different navigation msgs for a given epoch
-      nav_t nav_;
+inline void Navigation::freePclk(nav_t& nav)
+{
+  freeArray<pclk_t>(nav.pclk, nav.nc, nav.ncmax);
+}
 
-      // Private methods
+inline void Navigation::freeTec(nav_t& nav)
+{
+  freeArray<tec_t>(nav.tec, nav.nt, nav.ntmax);
+}
 
+inline void Navigation::freeFcb(nav_t& nav)
+{
+  freeArray<fcbd_t>(nav.fcb, nav.nf, nav.nfmax);
+}
 
-  };
+inline void Navigation::freeNavArrays(nav_t& nav)
+{
+  // RTKLIB "freenav(&nav_,255)" doesn't check if is NULL before freeing
+  freeEph(nav);
+  freeGeph(nav);
+  freeSeph(nav);
+  freePeph(nav);
+  freePclk(nav);
+  freeAlm(nav);
+  freeTec(nav);
+  freeFcb(nav);
 }
+
+}  // namespace GNSSUtils
 #endif
diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h
index e84820c2ab4dc86ef80ee79d06acc59da5047812..5610a7ec08698c19a6cf84152da0773b83f55cb2 100644
--- a/include/gnss_utils/observations.h
+++ b/include/gnss_utils/observations.h
@@ -2,55 +2,134 @@
 #define OBSERVATIONS_H
 
 #include <vector>
+#include <map>
 #include <iostream>
 #include <memory>
+#include <cassert>
 
 #include "gnss_utils/utils.h"
 
-extern "C"
-{
-    #include "rtklib.h"
+extern "C" {
+#include "rtklib.h"
 }
 
 namespace GNSSUtils
 {
-  class Observations
-  {
-    public:
-      // Constructor & Destructor
-      Observations();
-      ~Observations();
+class Observations;
+typedef std::shared_ptr<Observations>       ObservationsPtr;
+typedef std::shared_ptr<const Observations> ObservationsConstPtr;
+
+class Observations
+{
+public:
+  // Constructor & Destructor
+  Observations();
+  Observations(const Observations& obs);
+  ~Observations();
 
-      // Public objects
+  // Public objects
 
-      // Public methods
+  // Public methods
 
+  /* - Observations -  */
 
-      /* - Observations -  */
+  void clearObservations();
 
-      void clearObservations();
+  void addObservation(const obsd_t& obs);
+  void
+  loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt = 0.0, const char* opt = "");
 
-      void pushObservation(obsd_t & obs);
+  void removeObservationByIdx(const int& _idx);
+  void removeObservationBySat(const int& _sat);
 
-      void reserveObservations(unsigned int n);
+  std::vector<obsd_t>&       getObservations();
+  const std::vector<obsd_t>& getObservations() const;
 
-      const std::vector<obsd_t> & getObservations() const;
+  obsd_t&       getObservationBySat(const unsigned char& sat_number);
+  const obsd_t& getObservationBySat(const unsigned char& sat_number) const;
 
-      void print(obsd_t & _obs);
-      void print(int _obs_idx);
-      void print();
+  obsd_t&       getObservationByIdx(const int& idx);
+  const obsd_t& getObservationByIdx(const int& idx) const;
 
+  obsd_t*       data();
+  const obsd_t* data() const;
 
+  size_t size() const;
 
-    private:
-      // Private objects
-      // rtklib-like attribute to represent the different observation msgs for a given epoch
-      std::vector<obsd_t> obs_vector_;
+  bool hasSatellite(const unsigned char& i) const;
 
+  void print(obsd_t& _obs);
+  void printBySat(const int& _sat);
+  void printByIdx(const int& _idx);
+  void print();
 
-      // Private methods
+  static void findCommonObservations(const Observations& obs_1,
+                                     const Observations& obs_2,
+                                     Observations&       common_obs_1,
+                                     Observations&       common_obs_2);
 
+private:
+  // Private objects
+  std::map<unsigned char, int> sat_2_idx_;  //< key: corresponding sat number, value: idx in obs_ vector
+  std::vector<unsigned char>   idx_2_sat_;  //< key: idx in obs_ vector, value: corresponding sat number
+  std::vector<obsd_t>          obs_;        //< vector of RTKLIB observations for a given epoch
 
-  };
+  // Private methods
+};
+
+inline const std::vector<obsd_t>& Observations::getObservations() const
+{
+  return this->obs_;
+}
+
+inline std::vector<obsd_t>& Observations::getObservations()
+{
+  return this->obs_;
+}
+
+inline obsd_t& Observations::getObservationBySat(const unsigned char& sat_number)
+{
+  assert(sat_2_idx_.count(sat_number) != 0);
+  return obs_.at(sat_2_idx_.at(sat_number));
+}
+
+inline const obsd_t& Observations::getObservationBySat(const unsigned char& sat_number) const
+{
+  assert(sat_2_idx_.count(sat_number) != 0);
+  return obs_.at(sat_2_idx_.at(sat_number));
+}
+
+inline obsd_t& Observations::getObservationByIdx(const int& idx)
+{
+  assert(obs_.size() > idx);
+  return obs_.at(idx);
 }
+
+inline const obsd_t& Observations::getObservationByIdx(const int& idx) const
+{
+  assert(obs_.size() > idx);
+  return obs_.at(idx);
+}
+
+inline obsd_t* Observations::data()
+{
+  return obs_.data();
+}
+
+inline const obsd_t* Observations::data() const
+{
+  return obs_.data();
+}
+
+inline size_t Observations::size() const
+{
+  return obs_.size();
+}
+
+inline bool Observations::hasSatellite(const unsigned char& i) const
+{
+  return sat_2_idx_.count(i) != 0;
+}
+
+}  // namespace GNSSUtils
 #endif
diff --git a/include/gnss_utils/ublox_raw.h b/include/gnss_utils/ublox_raw.h
new file mode 100644
index 0000000000000000000000000000000000000000..c3e4cc15180559331f3b0732f4243eb684076b63
--- /dev/null
+++ b/include/gnss_utils/ublox_raw.h
@@ -0,0 +1,63 @@
+#ifndef UBLOX_RAW_H
+#define UBLOX_RAW_H
+
+#include "gnss_utils/gnss_utils.h"
+
+namespace GNSSUtils
+{
+enum RawDataType : int
+{
+  NO       = 0,
+  OBS      = 1,
+  NAV_EPH  = 2,
+  NAV_SBAS = 3,
+  NAV_ALM  = 9,
+  NAV_ANT  = 5,
+  NAV_DGPS = 7,
+  NAV_SSR  = 10,
+  NAV_LEX  = 31,
+  ERROR    = -1
+};
+
+class UBloxRaw
+{
+public:
+  UBloxRaw();
+  ~UBloxRaw();
+
+  RawDataType addDataStream(const std::vector<u_int8_t>& data_stream);
+  
+  const Observations& getObservations();
+  const Navigation& getNavigation();
+
+  RawDataType getRawDataType() const;
+
+private:
+  raw_t raw_data_;
+
+  Observations obs_;
+  Navigation   nav_;
+
+  RawDataType raw_data_type_;
+
+  void updateObservations();
+};
+
+inline const GNSSUtils::Observations& UBloxRaw::getObservations()
+{
+  return obs_;
+}
+
+inline const GNSSUtils::Navigation& UBloxRaw::getNavigation()
+{
+  return nav_;
+}
+
+inline RawDataType UBloxRaw::getRawDataType() const
+{
+  return raw_data_type_;
+}
+
+}  // namespace GNSSUtils
+
+#endif
diff --git a/include/gnss_utils/utils.h b/include/gnss_utils/utils.h
index 4d19e40ab78baa88c2d15a78184f7dafa23e357a..fa24b0896b8d0f4158143aae1c6a6698e66fe8d3 100644
--- a/include/gnss_utils/utils.h
+++ b/include/gnss_utils/utils.h
@@ -6,18 +6,16 @@
 #include <memory>
 #include <string>
 
-#define ARRAY_SIZE(arr) sizeof(arr)/sizeof(arr[0])
+#define ARRAY_SIZE(arr) sizeof(arr) / sizeof(arr[0])
 #define GNSSUTILS_MSG "--GNSSUtils--"
 
 namespace GNSSUtils
 {
-  
-
-  void print(std::string & _msg);
-  void printArray(std::string _name, int * _array, int size);
-  void printArray(std::string _name, unsigned char * _array, int size);
-  void printArray(std::string _name, double * _array, int size);
-  void printArray(std::string _name, float * _array, int size);
-}
+void print(std::string& _msg);
+void printArray(std::string _name, int* _array, int size);
+void printArray(std::string _name, unsigned char* _array, int size);
+void printArray(std::string _name, double* _array, int size);
+void printArray(std::string _name, float* _array, int size);
+}  // namespace GNSSUtils
 
 #endif
\ No newline at end of file
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
deleted file mode 100644
index bf6f31f6e439b57c78c3d7a9ed52da3c7462a125..0000000000000000000000000000000000000000
--- a/src/CMakeLists.txt
+++ /dev/null
@@ -1,62 +0,0 @@
-# rtklib path
-SET(RTKLIB_DIR ../deps/RTKLIB)
-SET(RTKLIB_SRC_DIR ${RTKLIB_DIR}/src)
-
-# driver source files
-SET(SOURCES
-    gnss_utils.cpp
-    observations.cpp
-    navigation.cpp
-    utils.cpp)
-
-SET(RTKLIB_SRC
-    ${RTKLIB_SRC_DIR}/pntpos.c
-    ${RTKLIB_SRC_DIR}/rtkcmn.c
-    ${RTKLIB_SRC_DIR}/sbas.c
-    ${RTKLIB_SRC_DIR}/ephemeris.c
-    ${RTKLIB_SRC_DIR}/preceph.c
-    ${RTKLIB_SRC_DIR}/qzslex.c
-    ${RTKLIB_SRC_DIR}/rtcm.c
-    ${RTKLIB_SRC_DIR}/rtcm2.c
-    ${RTKLIB_SRC_DIR}/rtcm3.c
-    ${RTKLIB_SRC_DIR}/rtcm3e.c
-    ${RTKLIB_SRC_DIR}/ionex.c
-    ${RTKLIB_SRC_DIR}/rinex.c)
-
-
-# application header files
-SET(HEADERS
-    ../include/gnss_utils/gnss_utils.h
-    ../include/gnss_utils/utils.h
-    ../include/gnss_utils/observations.h
-    ../include/gnss_utils/navigation.h
-  ${RTKLIB_SRC_DIR}/rtklib.h)
-
-# Eigen #######
-FIND_PACKAGE(Eigen3 REQUIRED)
-
-# Boost #######
-set(Boost_USE_STATIC_LIBS OFF)
-set(Boost_USE_MULTITHREADED OFF)
-set(Boost_USE_STATIC_RUNTIME OFF)
-FIND_PACKAGE(Boost REQUIRED)
-
-# Adding libraries' directories
-link_directories(/usr/lib/x86_64-linux-gnu/)
-
-# Adding include directories
-INCLUDE_DIRECTORIES(../include/ ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${RTKLIB_SRC_DIR})
-
-# create the shared library
-ADD_LIBRARY(gnss_utils SHARED ${SOURCES} ${RTKLIB_SRC})
-TARGET_LINK_LIBRARIES(gnss_utils ${Boost_LIBRARIES})
-
-# Installing
-INSTALL(TARGETS ${PROJECT_NAME}
-      RUNTIME DESTINATION bin
-      LIBRARY DESTINATION lib/iri-algorithms
-      ARCHIVE DESTINATION lib/iri-algorithms)
-INSTALL(FILES ${HEADERS} DESTINATION include/iri-algorithms/gnss_utils)
-INSTALL(FILES ../Findgnss_utils.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
-
-ADD_SUBDIRECTORY(examples)
diff --git a/src/TDCP.cpp b/src/TDCP.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..66e5a10997152c4544c7b6d1e5eaefa2cfa6028f
--- /dev/null
+++ b/src/TDCP.cpp
@@ -0,0 +1,579 @@
+/*
+ * TDCP.cpp
+ *
+ *  Created on: Dec 4, 2019
+ *      Author: jvallve
+ */
+
+#include "../include/gnss_utils/TDCP.h"
+
+namespace GNSSUtils
+{
+bool TDCP(const Observations& obs_r,
+          Navigation&         nav_r,
+          const Observations& obs_k,
+          const Navigation&   nav_k,
+          Eigen::Vector4d&    d,
+          Eigen::Matrix4d&    cov_d,
+          double&             residual,
+          std::set<int>&      discarded_sats,
+          const TDCPParams&   sd_params,
+          const prcopt_t&     opt)
+{
+  // COMPUTE SINGLE DIFFERENCES
+  return TDCP(obs_r,
+              nav_r,
+              computePos(obs_r, nav_r, opt).pos,
+              obs_k,
+              nav_k,
+              d,
+              cov_d,
+              residual,
+              discarded_sats,
+              sd_params,
+              opt);
+}
+
+bool TDCP(const Observations&    obs_r,
+          const Navigation&      nav_r,
+          const Eigen::Vector3d& x_r,
+          const Observations&    obs_k,
+          const Navigation&      nav_k,
+          Eigen::Vector4d&       d,
+          Eigen::Matrix4d&       cov_d,
+          double&                residual,
+          std::set<int>&         discarded_sats,
+          const TDCPParams&      sd_params,
+          const prcopt_t&        opt)
+{
+  // FIND COMMON SATELLITES OBSERVATIONS
+  Observations common_obs_r, common_obs_k;
+  Observations::findCommonObservations(obs_r, obs_k, common_obs_r, common_obs_k);
+  int n_common_sats = common_obs_r.getObservations().size();
+
+  // COMPUTE COMMON SATELLITES POSITION
+  std::map<int, Eigen::Vector3d> common_sats_pos_r, common_sats_pos_k;
+  computeSatellitesPositions(common_obs_r, nav_r, opt, common_sats_pos_r);
+  computeSatellitesPositions(common_obs_k, sd_params.use_old_nav ? nav_r : nav_k, opt, common_sats_pos_k);
+
+  // FILTER SATELLITES (ALREADY DISCARDED, CORRUPTED DATA, WRONG POSITION, CONSTELLATION, ELEVATION and SNR)
+  filterCommonSatellites(
+      common_obs_r, common_sats_pos_r, common_obs_k, common_sats_pos_k, discarded_sats, x_r, sd_params, opt);
+
+  // COMPUTE SINGLE DIFFERENCES
+  return TDCP(common_obs_r,
+              nav_r,
+              common_sats_pos_r,
+              x_r,
+              common_obs_k,
+              nav_k,
+              common_sats_pos_k,
+              d,
+              cov_d,
+              residual,
+              discarded_sats,
+              sd_params);
+}
+
+bool TDCP(const Observations&                   common_obs_r,
+          const Navigation&                     nav_r,
+          const std::map<int, Eigen::Vector3d>& common_sats_pos_r,
+          const Eigen::Vector3d&                x_r,
+          const Observations&                   common_obs_k,
+          const Navigation&                     nav_k,
+          const std::map<int, Eigen::Vector3d>& common_sats_pos_k,
+          Eigen::Vector4d&                      d,
+          Eigen::Matrix4d&                      cov_d,
+          double&                               residual,
+          std::set<int>&                        discarded_sats,
+          const TDCPParams&                     sd_params)
+{
+  assert(common_obs_r.size() == common_obs_k.size());
+  assert(common_obs_r.size() == common_sats_pos_r.size());
+  assert(common_obs_k.size() == common_sats_pos_k.size());
+
+  double tr(common_obs_r.getObservations().front().time.time + common_obs_r.getObservations().front().time.sec);
+  double tk(common_obs_k.getObservations().front().time.time + common_obs_k.getObservations().front().time.sec);
+
+  int required_n_sats(std::max(sd_params.min_common_sats + sd_params.raim_n, 4 + sd_params.raim_n));
+  int n_common_sats = common_sats_pos_k.size();
+
+  if (n_common_sats < required_n_sats)
+  {
+#if GNSS_UTILS_TDCP_DEBUG == 1
+    printf("TDCP: NOT ENOUGH COMMON SATS");
+    printf("TDCP: %i common sats available, %i sats are REQUIRED. [ SKIPPING TDCP ]", n_common_sats, required_n_sats);
+#endif
+    return false;
+  }
+
+  // MULTI-FREQUENCY
+  std::map<int, std::pair<int, int>> row_2_sat_freq;
+  int                                row = 0;
+  for (auto obs_idx = 0; obs_idx < common_obs_k.size(); obs_idx++)
+  {
+    auto&& obs_r = common_obs_r.getObservationByIdx(obs_idx);
+    auto&& obs_k = common_obs_k.getObservationByIdx(obs_idx);
+
+    const int& sat_number = obs_k.sat;
+
+    if (sd_params.use_carrier_phase)
+    {
+      // L1/E1
+      if (std::abs(obs_r.L[0]) > 1e-12 and std::abs(obs_k.L[0]) > 1e-12)
+      {
+        row_2_sat_freq[row] = std::make_pair(sat_number, 0);
+        row++;
+      }
+      if (!sd_params.use_multi_freq)
+        continue;
+
+      // L2 (GPS/GLO/QZS)
+      int sys = satsys(sat_number, NULL);
+      if (NFREQ >= 2 and (sys & (SYS_GPS | SYS_GLO | SYS_QZS)) and std::abs(obs_r.L[1]) > 1e-12 and
+          std::abs(obs_k.L[1]) > 1e-12)
+      {
+        row_2_sat_freq[row] = std::make_pair(sat_number, 1);
+        row++;
+      }
+      // E5 (GAL)
+      else if (NFREQ >= 3 and (sys & SYS_GAL) and std::abs(obs_r.L[2]) > 1e-12 and std::abs(obs_k.L[2]) > 1e-12)
+      {
+        row_2_sat_freq[row] = std::make_pair(sat_number, 2);
+        row++;
+      }
+    }
+    else
+        // L1/E1
+        if (std::abs(obs_r.P[0]) > 1e-12 and std::abs(obs_k.P[0]) > 1e-12)
+    {
+      row_2_sat_freq[row] = std::make_pair(sat_number, 0);
+      row++;
+    }
+  }
+  int n_differences = row_2_sat_freq.size();
+
+  // Initial guess
+  Eigen::Vector4d d_0(d), delta_d(Eigen::Vector4d::Zero());
+
+#if GNSS_UTILS_TDCP_DEBUG == 1
+  std::cout << "Receiver at t_r: " << x_r.transpose() << std::endl;
+  std::cout << "d initial guess: " << d_0.transpose() << std::endl;
+  std::cout << "common sats: ";
+  for (auto row_sat_freq_it : row_2_sat_freq)
+    std::cout << row_sat_freq_it.second.first << " ";
+  std::cout << std::endl;
+  std::cout << "discarded_sats: ";
+  for (auto sat : discarded_sats)
+    std::cout << sat << " ";
+  std::cout << std::endl;
+#endif
+
+  // FILL SATELLITES POSITIONS AND MEASUREMENTS =======================================================================
+  Eigen::Matrix<double, Eigen::Dynamic, 4> A(Eigen::Matrix<double, Eigen::Dynamic, 4>::Zero(n_differences, 4));
+  Eigen::VectorXd                          r(Eigen::VectorXd::Zero(n_differences));
+  Eigen::VectorXd                          drho_m(Eigen::VectorXd::Zero(n_differences));
+  Eigen::Matrix<double, 3, Eigen::Dynamic> s_k(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3, n_differences));
+  Eigen::Matrix<double, 3, Eigen::Dynamic> s_r(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3, n_differences));
+  int                                      sat_i = 0;
+
+  for (auto row_sat_freq_it : row_2_sat_freq)
+  {
+    const int& row        = row_sat_freq_it.first;
+    const int& sat_number = row_sat_freq_it.second.first;
+    const int& freq       = row_sat_freq_it.second.second;
+
+    auto obs_r = common_obs_r.getObservationBySat(sat_number);
+    auto obs_k = common_obs_k.getObservationBySat(sat_number);
+
+    // excluded satellite
+    if (discarded_sats.count(sat_number) != 0)
+    {
+#if GNSS_UTILS_TDCP_DEBUG == 1
+      std::cout << "\tdiscarded sat" << std::endl;
+#endif
+      continue;
+    }
+
+    // Satellite's positions
+    s_r.col(row) = common_sats_pos_r.at(sat_number);
+    s_k.col(row) = common_sats_pos_k.at(sat_number);
+    nav_k.getNavigation().ion_gps;
+
+    if (sd_params.use_carrier_phase)  // TODO: add iono and tropo corrections (optionally)
+      drho_m(row) = (obs_k.L[freq] * nav_k.getNavigation().lam[sat_number - 1][freq]) -
+                    (obs_r.L[freq] * nav_r.getNavigation().lam[sat_number - 1][freq]);
+    else
+      drho_m(row) = obs_k.P[freq] - obs_r.P[freq];
+
+#if GNSS_UTILS_TDCP_DEBUG == 1
+      // std::cout << "\tsat " << sat_number << " frequency " << freq << " wavelength = " <<
+      // nav_r.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\tpositions:\n\t\tt_r: " <<
+      // s_r.col(row).transpose() << "\n\t\tt_k: " << s_k.col(row).transpose() << std::endl; std::cout << "\tobs_r.L: "
+      // << obs_r.L[freq] << std::endl; std::cout << "\tobs_k.L: " << obs_k.L[freq] << std::endl; std::cout <<
+      // "\tnav_r.getNavigation().lam[sat_number-1][freq]: " << nav_r.getNavigation().lam[sat_number-1][freq] <<
+      // std::endl; std::cout << "\tnav_k.getNavigation().lam[sat_number-1][freq]: " <<
+      // nav_k.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\trho_r: " << obs_r.L[freq] *
+      // nav_r.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\trho_k: " << obs_k.L[freq] *
+      // nav_k.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\tdrho_m: " << drho_m(row) <<
+      // std::endl;
+#endif
+
+    if (!sd_params.relinearize_jacobian)
+    {
+      // Unit vectors from receivers to satellites
+      Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized();
+
+      // Fill Jacobian matrix
+      A(row, 0) = u_k(0);
+      A(row, 1) = u_k(1);
+      A(row, 2) = u_k(2);
+      A(row, 3) = -1.0;
+    }
+  }
+
+  // LEAST SQUARES SOLUTION =======================================================================
+  for (int j = 0; j < sd_params.max_iterations; j++)
+  {
+    // fill A and r
+    for (int row = 0; row < A.rows(); row++)
+    {
+      // Evaluate r
+      r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);
+
+      // Evaluate A
+      if (sd_params.relinearize_jacobian)
+      {
+        // Unit vectors from rcvrs to sats
+        Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized();
+
+        // Fill Jacobian matrix
+        A(row, 0) = u_k(0);
+        A(row, 1) = u_k(1);
+        A(row, 2) = u_k(2);
+        A(row, 3) = -1.0;
+      }
+    }
+
+    // Solve
+    cov_d   = (A.transpose() * A).inverse();
+    delta_d = -cov_d * A.transpose() * r;
+    d += delta_d;
+
+    // wrong solution
+    if (d.array().isNaN().any())
+    {
+      std::cout << "TDCP: LSM DID NOT WORK. NAN values appeared. ABORTING!!" << std::endl;
+      return false;
+    }
+
+    // residual
+    // residual = sqrt((r - A * delta_d).squaredNorm() / A.rows());
+    residual = (r + A * delta_d).norm();
+    // std::cout << "residual = " << residual << std::endl;
+    // std::cout << "TDCP2: r-A*delta_d = " << (r + A * delta_d).transpose() << std::endl;
+
+#if GNSS_UTILS_TDCP_DEBUG == 1
+    std::cout << "Displacement vector =" << d.head<3>().transpose() << std::endl;
+    std::cout << "Displacement update =" << delta_d.head<3>().transpose() << std::endl;
+    printf("Ref distance      = %7.3f m\n", d_0.norm());
+    printf("Computed distance = %7.3f m\n", d.head<3>().norm());
+    printf("TDCP: residual      = %13.10f\n", residual);
+    printf("TDCP: row           = %i\n", r.rows());
+    std::cout << "TDCP: drho          = " << r.transpose() << std::endl;
+    std::cout << "TDCP: drho_m        = " << drho_m.transpose() << std::endl;
+    std::cout << "TDCP: H             = \n" << A << std::endl;
+    printf("TDCP: dT            = %8.3f secs\n", d(3));
+#endif
+
+    // RAIM ====================================== (only at first iteration) RAIM WITH SATELLITES, not ROWS
+    int input_discarded_sats = discarded_sats.size();
+    if (j == 0 and sd_params.raim_n > 0 and residual > sd_params.raim_min_residual)
+    {
+      int             worst_sat_row = -1;
+      double          best_residual = 1e12;
+      Eigen::Vector4d best_d;
+      int             n_removed_rows = 1;
+
+      // remove some satellites
+      while (discarded_sats.size() - input_discarded_sats < sd_params.raim_n)
+      {
+        auto A_raim = A;
+        auto r_raim = r;
+
+        // solve removing each satellite
+        for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++)
+        {
+          int sat_removed = row_2_sat_freq.at(row_removed).first;
+
+          // Multi-freq: some rows for the same satellite
+          n_removed_rows = 1;
+          if (sd_params.use_multi_freq)
+            while (row_removed + n_removed_rows < A_raim.rows() and
+                   row_2_sat_freq.at(row_removed + n_removed_rows).first == sat_removed)
+              n_removed_rows++;
+
+          // remove satellite rows
+          for (auto r = 0; r < n_removed_rows; r++)
+          {
+            A_raim.row(row_removed + r) = Eigen::Vector4d::Zero().transpose();
+            r_raim(row_removed + r)     = 0;  // not necessary
+          }
+
+          // solve
+          Eigen::Vector4d delta_d_raim = -(A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim;
+          Eigen::Vector4d d_raim       = d_0 + delta_d_raim;
+
+          // no NaN
+          if (!d_raim.array().isNaN().any())
+          {
+            // residual
+            residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1);
+            // residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / A_raim.rows());
+
+#if GNSS_UTILS_TDCP_DEBUG == 1
+            std::cout << "RAIM excluding sat " << sat_removed << " - row " << row_removed << "(n_rows "
+                      << n_removed_rows << ")" << std::endl;
+            std::cout << "Displacement vector =" << d_raim.head<3>().transpose() << std::endl;
+            std::cout << "Displacement update =" << delta_d_raim.head<3>().transpose() << std::endl;
+            printf("Ref distance      = %7.3f m\n", d_0.norm());
+            printf("Computed distance = %7.3f m\n", d_raim.head<3>().norm());
+            printf("TDCP: residual      = %13.10f\n", residual);
+            std::cout << "TDCP: drho          = " << r_raim.transpose() << std::endl;
+            std::cout << "TDCP: H             = \n" << A_raim << std::endl;
+            printf("TDCP: dT            = %8.3f secs\n", d_raim(3));
+#endif
+            // store if best residual
+            if (residual < best_residual)
+            {
+              worst_sat_row = row_removed;
+              best_residual = residual;
+              best_d        = d_raim;
+            }
+          }
+          // restore initial A and r
+          for (auto row = 0; row < n_removed_rows; row++)
+          {
+            A_raim.row(row_removed + row) = A.row(row_removed + row);
+            r_raim(row_removed + row)     = r(row_removed + row);
+          }
+          row_removed += (n_removed_rows - 1);
+        }
+
+        // No successful RAIM solution
+        if (worst_sat_row == -1)
+        {
+          printf("TDCP: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!");
+          return false;
+        }
+
+        // store removed sat
+        discarded_sats.insert(row_2_sat_freq[worst_sat_row].first);
+
+        // decrease n_common_sats
+        n_differences -= n_removed_rows;
+        n_common_sats--;
+
+        // Remove selected satellite from problem
+        std::cout << "resizing problem..." << std::endl;
+        auto A_aux      = A;
+        auto r_aux      = r;
+        auto drho_m_aux = drho_m;
+        auto s_r_aux    = s_r;
+        auto s_k_aux    = s_k;
+        A.conservativeResize(n_differences, Eigen::NoChange);
+        r.conservativeResize(n_differences);
+        drho_m.conservativeResize(n_differences);
+        s_r.conservativeResize(Eigen::NoChange, n_differences);
+        s_k.conservativeResize(Eigen::NoChange, n_differences);
+
+        int back_elements_changing = A_aux.rows() - worst_sat_row - n_removed_rows;
+        if (back_elements_changing != 0)  // if last satelite removed, conservative resize did the job
+        {
+          A.bottomRows(back_elements_changing)  = A_aux.bottomRows(back_elements_changing);
+          s_r.rightCols(back_elements_changing) = s_r_aux.rightCols(back_elements_changing);
+          s_k.rightCols(back_elements_changing) = s_k_aux.rightCols(back_elements_changing);
+          r.tail(back_elements_changing)        = r_aux.tail(back_elements_changing);
+          drho_m.tail(back_elements_changing)   = drho_m_aux.tail(back_elements_changing);
+        }
+
+        // apply the RAIM solution
+        d     = best_d;
+        cov_d = (A.transpose() * A).inverse();
+
+#if GNSS_UTILS_TDCP_DEBUG == 1
+        std::cout << "TDCP After RAIM iteration" << std::endl;
+        std::cout << "\tExcluded sats : ";
+        for (auto dsat : discarded_sats)
+          std::cout << (int)dsat << " ";
+        std::cout << std::endl;
+        std::cout << "\tCommon sats   : " << n_common_sats << std::endl;
+        std::cout << "\tRows          : " << n_differences << std::endl;
+        std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
+        std::cout << "\tSolution      = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
+        std::cout << "\tClock offset  = " << d(3) << std::endl;
+        std::cout << "\tResidual      = " << best_residual << std::endl;
+#endif
+      }
+
+#if GNSS_UTILS_TDCP_DEBUG == 1
+      std::cout << "TDCP After RAIM " << std::endl;
+      std::cout << "\tExcluded sats : ";
+      for (auto dsat : discarded_sats)
+        std::cout << (int)dsat << " ";
+      std::cout << std::endl;
+      std::cout << "\tCommon sats   : " << n_common_sats << std::endl;
+      std::cout << "\tRows          : " << n_differences << std::endl;
+      std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
+      std::cout << "\tSolution      = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
+      std::cout << "\tClock offset  = " << d(3) << std::endl;
+      std::cout << "\tResidual      = " << best_residual << std::endl;
+#endif
+    }
+
+#if GNSS_UTILS_TDCP_DEBUG == 1
+    std::cout << "TDCP iteration " << j << std::endl;
+    std::cout << "\tExcluded sats : ";
+    for (auto dsat : discarded_sats)
+      std::cout << (int)dsat << " ";
+    std::cout << std::endl;
+    std::cout << "\tCommon sats   : " << n_common_sats << std::endl;
+    std::cout << "\tRows          : " << n_differences << std::endl;
+    std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
+    std::cout << "\tSolution      = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
+    std::cout << "\tClock offset  = " << d(3) << std::endl;
+    std::cout << "\tResidual      = " << residual << std::endl;
+#endif
+  }
+
+  // weight covariance with the measurement noise (proportional to time)
+  double sq_sigma_TDCP = (tk - tr) * sd_params.sigma_atm * sd_params.sigma_atm +
+                         2 * (sd_params.use_carrier_phase ? sd_params.sigma_carrier * sd_params.sigma_carrier :
+                                                            sd_params.sigma_code * sd_params.sigma_code);
+  cov_d *= sq_sigma_TDCP;
+  // residual = (r - A * d).norm() / A.rows();
+
+  // Computing error on measurement space
+  for (int row = 0; row < n_differences; row++)
+  {
+    r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);
+  }
+  residual = sqrt(r.squaredNorm() / r.size());
+
+  return true;
+}
+
+void filterCommonSatellites(Observations&                   common_obs_r,
+                            std::map<int, Eigen::Vector3d>& common_sats_pos_r,
+                            Observations&                   common_obs_k,
+                            std::map<int, Eigen::Vector3d>& common_sats_pos_k,
+                            std::set<int>&                  discarded_sats,
+                            const Eigen::Vector3d&          x_r,
+                            const TDCPParams&               sd_params,
+                            const prcopt_t&                 opt)
+{
+  assert(&common_obs_r != &common_obs_k);
+  assert(&common_sats_pos_r != &common_sats_pos_k);
+  assert(common_obs_r.size() == common_obs_k.size());
+  assert(common_obs_r.size() == common_sats_pos_r.size());
+  assert(common_obs_r.size() == common_sats_pos_k.size());
+
+  // std::cout << "filterCommonSatellites: initial size: " << common_obs_k.size() << std::endl;
+
+  std::set<int> remove_sats;
+
+  for (int obs_i = 0; obs_i < common_obs_r.size(); obs_i++)
+  {
+    auto&& obs_r = common_obs_r.getObservationByIdx(obs_i);
+    auto&& obs_k = common_obs_k.getObservationByIdx(obs_i);
+    assert(obs_r.sat == obs_k.sat && "common satellites observations have to be ordered equally");
+    const int& sat_number = obs_r.sat;
+
+    // already discarded sats
+    if (discarded_sats.count(sat_number) != 0)
+    {
+      remove_sats.insert(sat_number);
+      continue;
+    }
+
+    // wrong data (satellite is not included in the discarded list)
+    if (sd_params.use_carrier_phase and (std::abs(obs_r.L[0]) < 1e-12 or std::abs(obs_k.L[0]) < 1e-12))
+    {
+      // std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_r.L[0] << " " <<
+      // obs_k.L[0] << std::endl;
+      remove_sats.insert(sat_number);
+      continue;
+    }
+    if (!sd_params.use_carrier_phase and (std::abs(obs_r.P[0]) < 1e-12 or std::abs(obs_k.L[0]) < 1e-12))
+    {
+      // std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_r.P[0] << " " <<
+      // obs_k.P[0] << std::endl;
+      remove_sats.insert(sat_number);
+      continue;
+    }
+
+    // bad satellite position (satellite is not included in the discarded list)
+    if (common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3) or
+        common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3))
+    {
+      // std::cout << "Discarding sat " << sat_number << ": bad satellite position: \n\t" <<
+      // common_sats_pos_r[sat_number].transpose() << "\n\t" << common_sats_pos_k[sat_number].transpose() << std::endl;
+      remove_sats.insert(sat_number);
+      continue;
+    }
+
+    // constellation
+    int sys = satsys(obs_r.sat, NULL);
+    if (!(sys & opt.navsys))
+    {
+      // std::cout << "Discarding sat " << sat_number << ": not selected constellation " << sys << " - mask: " <<
+      // opt.navsys << std::endl;
+      discarded_sats.insert(sat_number);
+      remove_sats.insert(sat_number);
+      continue;
+    }
+
+    // check both elevations
+    double elevation = std::min(computeSatElevation(x_r, common_sats_pos_r[sat_number]),
+                                computeSatElevation(x_r, common_sats_pos_k[sat_number]));
+    if (elevation < opt.elmin)
+    {
+      // std::cout << "Discarding sat " << sat_number << ": low elevation " << elevation << " - min: " << opt.elmin <<
+      // std::endl;
+      discarded_sats.insert(sat_number);
+      remove_sats.insert(sat_number);
+      continue;
+    }
+
+    // snr TODO: multifrequency (2nd param and 3rd idx)
+    if (testsnr(0, 0, elevation, obs_r.SNR[0] * 0.25, &opt.snrmask) == 1)
+    {
+      // std::cout << "Discarding sat " << sat_number << ": snr test " << std::endl;
+      discarded_sats.insert(sat_number);
+      remove_sats.insert(sat_number);
+      continue;
+    }
+  }
+
+  // remove sats
+  // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+  for (auto sat : remove_sats)
+  {
+    assert(common_obs_r.hasSatellite(sat));
+    assert(common_obs_k.hasSatellite(sat));
+    assert(common_sats_pos_r.count(sat));
+    assert(common_sats_pos_k.count(sat));
+    common_obs_r.removeObservationBySat(sat);
+    common_obs_k.removeObservationBySat(sat);
+    common_sats_pos_r.erase(sat);
+    common_sats_pos_k.erase(sat);
+  }
+
+  assert(common_obs_r.size() == common_obs_k.size());
+  assert(common_obs_r.size() == common_sats_pos_r.size());
+  assert(common_obs_r.size() == common_sats_pos_k.size());
+
+  // std::cout << "final size: " << common_obs_k.size() << std::endl;
+}
+
+}  // namespace GNSSUtils
diff --git a/src/examples/gnss_utils_test.cpp b/src/examples/gnss_utils_test.cpp
index fd5589b48b65ae24c960a19b75900cd8c419cc00..814ee443c905e3a1fd8d327bcad4d096227a02f6 100644
--- a/src/examples/gnss_utils_test.cpp
+++ b/src/examples/gnss_utils_test.cpp
@@ -1,5 +1,6 @@
 #include "gnss_utils/observations.h"
 #include "gnss_utils/navigation.h"
+#include "gnss_utils/gnss_utils.h"
 
 #include <typeinfo>
 
@@ -7,146 +8,44 @@
 
 #include <iostream>
 
-extern "C"
-{
-  #include "../deps/RTKLIB/src/rinex.c"
+extern "C" {
+#include "../deps/RTKLIB/src/rinex.c"
 }
 
 using namespace GNSSUtils;
 
-
-int createObsAndNav(Observations* observations, char* obs_path,  Navigation* navigation, char* nav_path)
-{
-  // Time
-  double ts_d[] = {2018, 5, 17, 14, 1, 2};
-  gtime_t ts = epoch2time(ts_d);
-
-  double te_d[] = {2018, 5, 17, 14, 42, 8};
-  gtime_t te = epoch2time(te_d);
-
-  double tint = 0.1;
-
-  // Create options container
-  char* opt = "-SYS=G";
-
-  // Create receiver identifier
-  int rcv = 1;
-
-  // Create object types
-  rnxopt_t rnx_opt;
-
-  // Create obsd_t object to hold the observations
-  obs_t obs;
-  obs.data = (obsd_t *) malloc(sizeof(obsd_t)*MAXSAT);
-
-  /* header */
-  std::cout << "------------" << std::endl;
-  std::cout << "Observations" << std::endl;
-  std::cout << "------------" << std::endl;
-
-  if (readrnx(obs_path, rcv, opt, &obs, NULL, NULL))
-  {
-    sortobs(&obs);
-    obs.data[0].P[0] = 21597659.275;
-    obs.data[1].P[0] = 22348322.550;
-    obs.data[2].P[0] = 20873167.574;
-    obs.data[3].P[0] = 20920707.830;
-
-    std::cout << "Obs number: " << obs.n << std::endl;
-
-
-  }
-
-
-  else
-  {
-    std::cout << "Couldn't load provided observation file" << std::endl;
-    return 0;
-  }
-
-
-
-  for (int i=0; i < obs.n; i++)
-  {
-    std::cout << "time: " << time_str(obs.data[i].time, 3) <<  " | sat: " << int(obs.data[i].sat) << " | rcv: " << obs.data[i].rcv <<
-               " | SNR: " << obs.data[i].SNR[0] << " | LLI: " << obs.data[i].LLI[0] << " | code: " << obs.data[i].code[0] <<
-                 " | L: " << obs.data[i].L[0] <<  " | P: " << obs.data[i].P[0] << " | D: " << obs.data[i].D[0] << std::endl;
-    observations->pushObservation(obs.data[i]);
-  }
-
-  free(obs.data);
-
-  /* header */
-  std::cout << "----------" << std::endl;
-  std::cout << "Navigation" << std::endl;
-  std::cout << "----------" << std::endl;
-
-  /* Load _nav attribute into nav variable */
-  nav_t nav = navigation->getNavigation();
-
-
-  if (readrnx(nav_path, rcv, opt, NULL, &nav, NULL))
-  {
-    std::cout << "Navigation file loaded." << std::endl;
-    std::cout << "GPS satellites in navigation file: " << nav.n << std::endl;
-    std::cout << "GLONASS satellites in navigation file: " << nav.ng << std::endl;
-    std::cout << "SBAS satellites in navigation file: " << nav.ns << std::endl;
-    std::cout << "Almanac satellites in navigation file: " << nav.na << std::endl;
-    uniqnav(&nav);
-  }
-
-
-  else
-  {
-    std::cout << "Couldn't load provided navigation file" << std::endl;
-    return 0;
-  }
-
-
-  /* Print Constellation Satellites ids */
-  for (int i=0;i<nav.n;i++)
-  {
-    std::cout <<  i << " GPS Sat Id: " << nav.eph[i].sat << std::endl;
-  }
-
-  for (int i=0;i<nav.ng;i++)
-  {
-    std::cout <<  i << " GLONASS Sat Id: " << nav.geph[i].sat << std::endl;
-  }
-
-  for (int i=0;i<nav.ns;i++)
-  {
-    std::cout <<  i << " SBAS Sat Id: " << nav.seph[i].sat << std::endl;
-  }
-
-
-  /* Load nav into Navigation object*/
-  navigation->setNavigation(nav);
-
-  std::cout << "Navigation message loaded to Navigation class." << std::endl;
-
-  return 1;
-}
-
-int main(int argc, char *argv[])
+int main(int argc, char* argv[])
 {
   /* header */
   std::cout << "--------------------------" << std::endl;
   std::cout << "GNSS Utils Library Example" << std::endl;
   std::cout << "--------------------------" << std::endl;
 
-
   // create Observations object
   Observations observations;
 
   // create Navigation object
   Navigation navigation;
 
-  createObsAndNav(&observations, "../src/examples/raw_201805171357.obs", &navigation, "../src/examples/raw_201805171357.nav");
+  gtime_t     t_start{ 0, 0 };  // no limit
+  gtime_t     t_end{ 0, 0 };    // no limit
+  double      dt  = 0.0;        // no limit
+  const char* opt = "";         // only GPS | GPS+GAL: "-SYS=G,L" | ALL: ""
 
-  nav_t nav = navigation.getNavigation();
+  // RTKLIB trace
+  // char str_file[80];
+  // snprintf(str_file, sizeof str_file, "../src/examples/trace");
 
-  std::cout << "Navigation eph number " << nav.n << std::endl;
+  // load observations from RINEX file
+  observations.loadFromRinex("../src/examples/sample_data.obs", t_start, t_end, dt, opt);
+  observations.print();
+
+  // Trace close
+  // traceclose();
+
+  // load navigation from RINEX file
+  navigation.loadFromRinex("../src/examples/sample_data.nav", t_start, t_end, dt, opt);
+  navigation.print();
 
   /* Set processing options */
 
@@ -156,25 +55,26 @@ int main(int argc, char *argv[])
   std::cout << "------------------" << std::endl;
 
   prcopt_t prcopt = prcopt_default;
-  prcopt.mode = PMODE_SINGLE;
-  prcopt.soltype = 0;
-  prcopt.nf = 1;
-  prcopt.navsys = SYS_GPS;
-  //prcopt.elmin = 1.05;  // 60 degrees = 1.05 rad
-  prcopt.sateph = EPHOPT_BRDC;
-  prcopt.ionoopt = IONOOPT_OFF;
-  prcopt.tropopt = TROPOPT_OFF;
+  prcopt.mode     = PMODE_SINGLE;
+  prcopt.soltype  = 0;
+  prcopt.nf       = 1;
+  prcopt.navsys   = SYS_GPS;
+  prcopt.elmin    = 0.525;  // 60 degrees = 1.05 rad
+  prcopt.sateph   = EPHOPT_BRDC;
+  prcopt.ionoopt  = IONOOPT_OFF;
+  prcopt.tropopt  = TROPOPT_OFF;
   prcopt.dynamics = 0;
   prcopt.tidecorr = 0;
   prcopt.sbascorr = SBSOPT_FCORR;
-  prcopt.ru[0] = 4789374.0336;
-  prcopt.ru[1] = 177048.3292;
-  prcopt.ru[2] = 4194542.6444;
+  prcopt.ru[0]    = 0;  // 4789374.0336;
+  prcopt.ru[1]    = 0;  // 177048.3292;
+  prcopt.ru[2]    = 0;  // 4194542.6444;
 
   std::cout << "Processing options defined" << std::endl;
 
+  Eigen::Vector3d lla_gt(41.383293114 * M_PI / 180.0, 2.116101115 * M_PI / 180.0, -91.6641);
 
-  //Compute spp
+  // Compute spp
 
   /* header */
   std::cout << "-----------" << std::endl;
@@ -183,27 +83,19 @@ int main(int argc, char *argv[])
 
   int stat;
 
-  sol_t solb={{0}};
-
-  char msg[128]="";
+  sol_t solb = { { 0 } };
 
-  std::vector<obsd_t> obs = observations.getObservations();
+  char msg[128] = "";
 
-  for (int i=0; i < obs.size(); i++)
-  {
-    std::cout << "time: " << time_str(obs[i].time, 3) <<  " | sat: " << int(obs[i].sat) << " | rcv: " << obs[i].rcv <<
-               " | SNR: " << obs[i].SNR[0] << " | LLI: " << obs[i].LLI[0] << " | code: " << obs[i].code[0] <<
-                 " | L: " << obs[i].L[0] <<  " | P: " << obs[i].P[0] << " | D: " << obs[i].D[0] << std::endl;
-  }
-
-  std::cout << "obs.size(): " << obs.size() << std::endl;
-
-  stat = pntpos(&obs[0], obs.size(), &nav, &prcopt, &solb, NULL, NULL, msg);
+  stat = pntpos(observations.data(), observations.size(), &navigation.getNavigation(), &prcopt, &solb, NULL, NULL, msg);
 
   std::cout << "msg: " << msg << std::endl;
-
-  std::cout << "sol.stat: " << solb.stat << std::endl;
-
-  std::cout << "Position: " << solb.rr[0] << ", " << solb.rr[1] << ", " << solb.rr[2] << 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): " << 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;
+
+  traceclose();
 }
diff --git a/src/examples/sample_data.nav b/src/examples/sample_data.nav
new file mode 100644
index 0000000000000000000000000000000000000000..5c4c418dc876fd45e7fddca388b4cc98b0202441
--- /dev/null
+++ b/src/examples/sample_data.nav
@@ -0,0 +1,406 @@
+     3.04           N: GNSS NAV DATA    M: MIXED            RINEX VERSION / TYPE
+sbf2rin-13.4.3                          20200205 113410 UTC PGM / RUN BY / DATE 
+GPSA   9.3132E-09 -1.4901E-08 -5.9605E-08  1.1921E-07       IONOSPHERIC CORR    
+GPSB   9.6256E+04 -1.4746E+05 -1.3107E+05  9.1750E+05       IONOSPHERIC CORR    
+GAL    2.9250E+01  3.8281E-01  3.2959E-03  0.0000E+00       IONOSPHERIC CORR    
+GPUT -9.3132257462E-10 8.881784197E-16 503808 2083          TIME SYSTEM CORR    
+GAUT  9.3132257462E-10 0.000000000E+00 345600 2083          TIME SYSTEM CORR    
+GAGP  1.2514647096E-09-5.329070518E-15 345600 2083          TIME SYSTEM CORR    
+    18                                                      LEAP SECONDS        
+                                                            END OF HEADER       
+G02 2019 12 12 12 00 00-3.648423589766E-04-7.503331289627E-12 0.000000000000E+00
+     4.700000000000E+01-1.208437500000E+02 4.378039505776E-09-1.891689129337E+00
+    -6.053596735001E-06 1.953727600630E-02 9.275972843170E-06 5.153594808578E+03
+     3.888000000000E+05 3.371387720108E-07-2.768163608668E-01 2.346932888031E-07
+     9.575379914494E-01 1.915312500000E+02-1.691294764827E+00-7.654961717025E-09
+     2.000083311498E-11 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+     2.000000000000E+00 0.000000000000E+00-1.769512891769E-08 4.700000000000E+01
+     3.868920000000E+05 4.000000000000E+00
+G04 2019 12 12 12 00 00-1.993030309677E-05-5.002220859751E-12 0.000000000000E+00
+     1.570000000000E+02-1.387500000000E+01 4.959135139313E-09-2.339868507292E+00
+    -8.419156074524E-07 1.249722088687E-03 5.329027771950E-06 5.150576126099E+03
+     3.888000000000E+05 5.587935447693E-09 1.915372002909E+00 1.303851604462E-08
+     9.594040575671E-01 2.769062500000E+02-1.281079249179E+00-8.132124449911E-09
+    -3.685867816904E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+     4.096000000000E+03 6.300000000000E+01-4.190951585770E-09 6.690000000000E+02
+     3.873960000000E+05 4.000000000000E+00
+G06 2019 12 12 12 00 00-1.512947492301E-04-1.114131009672E-11 0.000000000000E+00
+     1.000000000000E+02-1.239062500000E+02 3.927663602954E-09-1.824134113525E+00
+    -6.347894668579E-06 1.658447668888E-03 9.786337614059E-06 5.153719285965E+03
+     3.888000000000E+05-1.490116119385E-08-2.122724576342E-01 5.401670932770E-08
+     9.781077241470E-01 2.010000000000E+02-1.200768673201E+00-7.640318249923E-09
+     1.214336296267E-11 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+     2.000000000000E+00 0.000000000000E+00 4.656612873077E-09 1.000000000000E+02
+     3.858180000000E+05 4.000000000000E+00
+G07 2019 12 12 12 00 00-1.683332957327E-04-8.185452315956E-12 0.000000000000E+00
+     5.700000000000E+01 3.909375000000E+01 4.250534194668E-09-2.882882750792E+00
+     1.888722181320E-06 1.320131728426E-02 1.172721385956E-05 5.153718780518E+03
+     3.888000000000E+05-7.450580596924E-09 2.943673467473E+00-1.341104507446E-07
+     9.545190186113E-01 1.482812500000E+02-2.422380694519E+00-7.540671242082E-09
+    -4.639478967207E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+     2.000000000000E+00 0.000000000000E+00-1.117587089539E-08 5.700000000000E+01
+     3.858180000000E+05 4.000000000000E+00
+G09 2019 12 12 12 00 00-1.062308438122E-04-9.436007530894E-12 0.000000000000E+00
+     7.800000000000E+01-1.206250000000E+01 5.168786729286E-09 4.992301981873E-01
+    -7.078051567078E-07 1.692383317277E-03 5.021691322327E-06 5.153534730911E+03
+     3.888000000000E+05-2.793967723846E-08 1.868752621939E+00 1.490116119385E-08
+     9.520568016730E-01 2.776875000000E+02 1.684705661839E+00-8.283559329210E-09
+    -2.914407111040E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+     2.000000000000E+00 0.000000000000E+00 1.396983861923E-09 7.800000000000E+01
+     3.857520000000E+05 4.000000000000E+00
+G13 2019 12 12 12 00 00-2.714386209846E-05 2.387423592154E-12 0.000000000000E+00
+     8.700000000000E+01-1.987500000000E+01 4.779841956746E-09-8.793798521920E-01
+    -1.072883605957E-06 4.130274290219E-03 5.709007382393E-06 5.153656044006E+03
+     3.888000000000E+05-6.332993507385E-08 2.009100851183E+00 9.313225746155E-08
+     9.674158381471E-01 2.706562500000E+02 1.183260297004E+00-8.083193840326E-09
+    -5.078782980268E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+     2.000000000000E+00 0.000000000000E+00-1.117587089539E-08 8.700000000000E+01
+     3.870420000000E+05 4.000000000000E+00
+G23 2019 12 12 12 00 00-1.500290818512E-04 2.387423592154E-12 0.000000000000E+00
+     3.000000000000E+01-1.009375000000E+01 5.335222233421E-09-1.452196357053E+00
+    -4.973262548447E-07 1.326873130165E-02 4.552304744720E-06 5.153694892883E+03
+     3.888000000000E+05 2.980232238770E-07 1.861752114203E+00 4.470348358154E-08
+     9.428683609718E-01 2.778750000000E+02-2.215284453592E+00-8.261415549690E-09
+    -2.478674675321E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+     2.000000000000E+00 0.000000000000E+00-2.048909664154E-08 3.000000000000E+01
+     3.858420000000E+05 4.000000000000E+00
+G30 2019 12 12 12 00 00-1.042019575834E-04-8.640199666843E-12 0.000000000000E+00
+     2.500000000000E+01 4.368750000000E+01 4.663408535398E-09-2.906933186921E+00
+     2.210959792137E-06 4.164319136180E-03 1.142919063568E-05 5.153722436905E+03
+     3.888000000000E+05-2.793967723846E-08 2.968865819418E+00-3.725290298462E-09
+     9.399056183160E-01 1.480312500000E+02-2.921787544453E+00-7.888185717455E-09
+    -4.778770483544E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+     2.000000000000E+00 0.000000000000E+00 3.725290298462E-09 2.500000000000E+01
+     3.858180000000E+05 4.000000000000E+00
+R03 2019 12 12 11 15 00 6.369315087795E-06 0.000000000000E+00 3.858000000000E+05
+     9.652274414063E+03 3.457994461060E-01 9.313225746155E-10 0.000000000000E+00
+     1.708528710938E+04 2.202743530273E+00-1.862645149231E-09 5.000000000000E+00
+     1.629986035156E+04-2.522974967957E+00-2.793967723846E-09 0.000000000000E+00
+R03 2019 12 12 11 45 00 6.371177732944E-06 0.000000000000E+00 3.870000000000E+05
+     1.043946044922E+04 4.914197921753E-01 9.313225746155E-10 0.000000000000E+00
+     2.039817236328E+04 1.453030586243E+00-1.862645149231E-09 5.000000000000E+00
+     1.118693066406E+04-3.121288299561E+00-1.862645149231E-09 0.000000000000E+00
+R05 2019 12 12 11 15 00 4.108343273401E-05 9.094947017729E-13 3.862800000000E+05
+     5.985222167969E+03 1.195173263550E-01-1.862645149231E-09 0.000000000000E+00
+    -1.828012353516E+04 2.295486450195E+00 1.862645149231E-09 1.000000000000E+00
+     1.676849218750E+04 2.457766532898E+00-1.862645149231E-09 0.000000000000E+00
+R05 2019 12 12 11 45 00 4.108529537916E-05 9.094947017729E-13 3.871800000000E+05
+     6.598400390625E+03 5.781021118164E-01-9.313225746155E-10 0.000000000000E+00
+    -1.370180371094E+04 2.745733261108E+00 9.313225746155E-10 1.000000000000E+00
+     2.048785058594E+04 1.648019790649E+00-1.862645149231E-09 0.000000000000E+00
+R14 2019 12 12 11 15 00 4.597380757332E-05 0.000000000000E+00 3.858000000000E+05
+     2.135816357422E+04-6.865911483765E-01 3.725290298462E-09 0.000000000000E+00
+     1.261080761719E+04-4.694700241089E-01-0.000000000000E+00-7.000000000000E+00
+     5.991423828125E+03 3.442845344543E+00-2.793967723846E-09 0.000000000000E+00
+R14 2019 12 12 11 45 00 4.597473889589E-05 0.000000000000E+00 3.870000000000E+05
+     1.936734765625E+04-1.519350051880E+00 1.862645149231E-09 0.000000000000E+00
+     1.162212011719E+04-5.869359970093E-01-9.313225746155E-10-7.000000000000E+00
+     1.187716162109E+04 3.054467201233E+00-2.793967723846E-09 0.000000000000E+00
+R15 2019 12 12 11 15 00 1.051910221577E-04 0.000000000000E+00 3.860100000000E+05
+     2.249286962891E+04 1.470921516418E+00 5.587935447693E-09 0.000000000000E+00
+     4.179755859375E+03 4.950771331787E-01 9.313225746155E-10 0.000000000000E+00
+    -1.125261865234E+04 3.118181228638E+00-1.862645149231E-09 0.000000000000E+00
+R15 2019 12 12 11 45 00 1.051910221577E-04 0.000000000000E+00 3.870000000000E+05
+     2.451276953125E+04 7.425718307495E-01 4.656612873077E-09 0.000000000000E+00
+     4.643450683594E+03 4.951381683350E-02 0.000000000000E+00 0.000000000000E+00
+    -5.276882812500E+03 3.478320121765E+00-2.793967723846E-09 0.000000000000E+00
+R19 2019 12 12 11 15 00-6.853323429823E-05-1.818989403546E-12 3.859500000000E+05
+     2.008951708984E+04 2.060537338257E-01 2.793967723846E-09 0.000000000000E+00
+    -1.541425244141E+04-4.377956390381E-01 2.793967723846E-09 3.000000000000E+00
+     3.049126953125E+03-3.578741073608E+00-1.862645149231E-09 0.000000000000E+00
+R19 2019 12 12 11 45 00-6.853695958853E-05-1.818989403546E-12 3.870000000000E+05
+     1.978871044922E+04-5.135507583618E-01 3.725290298462E-09 0.000000000000E+00
+    -1.571828076172E+04 1.333026885986E-01 1.862645149231E-09 3.000000000000E+00
+    -3.427280273438E+03-3.570507049561E+00-1.862645149231E-09 0.000000000000E+00
+R13 2019 12 12 11 45 00-3.215298056602E-05-0.000000000000E+00 3.876000000000E+05
+     3.100402832031E+03-2.939124107361E+00-1.862645149231E-09 0.000000000000E+00
+     1.186301953125E+04-8.695030212402E-01-9.313225746155E-10-2.000000000000E+00
+     2.235348193359E+04 8.685455322266E-01-1.862645149231E-09 0.000000000000E+00
+R20 2019 12 12 11 45 00-3.986386582255E-04-0.000000000000E+00 3.877800000000E+05
+     9.837636718750E+03 1.428291320801E+00-0.000000000000E+00 0.000000000000E+00
+    -1.795626220703E+04-1.534766197205E+00 1.862645149231E-09 2.000000000000E+00
+     1.525312060547E+04-2.726587295532E+00-1.862645149231E-09 0.000000000000E+00
+E01 2019 12 12 11 10 00-7.507699192502E-04-7.986500349944E-12 0.000000000000E+00
+     3.000000000000E+00 2.013125000000E+02 2.449744898851E-09 7.981291655576E-01
+     9.473413228989E-06 1.731918891892E-04 9.533017873764E-06 5.440622966766E+03
+     3.858000000000E+05 3.166496753693E-08-2.605880888987E+00-1.117587089539E-08
+     9.855193908613E-01 1.463750000000E+02-1.069210219000E+00-5.272719629937E-09
+     1.914365455291E-10 5.170000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00-1.629814505577E-09-1.862645149231E-09
+     3.864650000000E+05
+E01 2019 12 12 11 20 00-7.507746340707E-04-7.972289495228E-12 0.000000000000E+00
+     4.000000000000E+00 2.002500000000E+02 2.449744898851E-09 8.736282654153E-01
+     9.419396519661E-06 1.731686061248E-04 9.588897228241E-06 5.440622581482E+03
+     3.864000000000E+05 3.725290298462E-08-2.605884063520E+00-1.862645149231E-09
+     9.855195488564E-01 1.452500000000E+02-1.070324887968E+00-5.265933632987E-09
+     1.896507568581E-10 5.170000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00-1.629814505577E-09-1.862645149231E-09
+     3.870650000000E+05
+E09 2019 12 12 11 10 00 6.223833654076E-03-1.216449163621E-11 0.000000000000E+00
+     3.000000000000E+00 7.218750000000E+00 3.823373544569E-09-3.442271110123E-02
+     3.892928361893E-07 2.964780433103E-04 3.403052687645E-06 5.440603132248E+03
+     3.858000000000E+05-9.313225746155E-09 1.575307486314E+00-6.146728992462E-08
+     9.586413959478E-01 2.625000000000E+02 2.794206052069E-01-5.815242228181E-09
+     8.571785620706E-12 5.170000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00-4.656612873077E-10-6.984919309616E-10
+     3.869030000000E+05
+E12 2019 12 12 10 40 00 6.055364210624E-03-1.870148480521E-11 0.000000000000E+00
+     0.000000000000E+00-2.075312500000E+02 2.603322724555E-09-2.414195903395E+00
+    -9.659677743912E-06 2.996901748702E-04 6.495043635368E-06 5.440614477158E+03
+     3.840000000000E+05-2.421438694000E-08-5.219517721978E-01 5.774199962616E-08
+     9.864495053986E-01 2.151250000000E+02-9.348391204646E-01-5.376652530588E-09
+    -2.257236880119E-10 5.170000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00-1.327134668827E-08-1.327134668827E-08
+     3.858050000000E+05
+E19 2019 12 12 11 00 00-2.469634637237E-06 2.273736754432E-13 0.000000000000E+00
+     2.000000000000E+00 1.103125000000E+01 3.765871149364E-09-1.685770007887E-01
+     4.824250936508E-07 1.212444622070E-04 3.688037395477E-06 5.440600940704E+03
+     3.852000000000E+05-5.401670932770E-08 1.580615112564E+00 5.215406417847E-08
+     9.583897608719E-01 2.595937500000E+02 1.996512379669E+00-5.770954669140E-09
+     1.160762636137E-10 5.170000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00-5.820766091347E-09-5.820766091347E-09
+     3.858660000000E+05
+E19 2019 12 12 11 10 00-2.469576429576E-06 2.273736754432E-13 0.000000000000E+00
+     3.000000000000E+00 1.168750000000E+01 3.765156833895E-09-9.388288915917E-02
+     5.066394805908E-07 1.211694907397E-04 3.693625330925E-06 5.440600774765E+03
+     3.858000000000E+05-5.960464477539E-08 1.580611582543E+00 4.097819328308E-08
+     9.583898398694E-01 2.594687500000E+02 1.996204499781E+00-5.764168672191E-09
+     1.207193141583E-10 5.170000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00-5.820766091347E-09-5.820766091347E-09
+     3.864650000000E+05
+E21 2019 12 12 11 10 00-5.709171527997E-04-2.131628207280E-12 0.000000000000E+00
+     3.000000000000E+00 2.180312500000E+02 2.333311477503E-09 1.537835498027E+00
+     1.022964715958E-05 9.841390419751E-05 9.685754776001E-06 5.440636165619E+03
+     3.858000000000E+05-2.980232238770E-08-2.608584029515E+00 4.842877388000E-08
+     9.850865499746E-01 1.424062500000E+02-1.025413854454E+00-5.217717338871E-09
+     1.871506527187E-10 5.170000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 2.328306436539E-10
+     3.867350000000E+05
+E21 2019 12 12 11 20 00-5.709183751605E-04-2.131628207280E-12 0.000000000000E+00
+     4.000000000000E+00 2.213125000000E+02 2.317953694933E-09 1.607657241034E+00
+     1.038052141666E-05 9.810912888497E-05 9.816139936447E-06 5.440637243271E+03
+     3.864000000000E+05-4.097819328308E-08-2.608587130902E+00 4.470348358154E-08
+     9.850867050439E-01 1.395312500000E+02-1.020851268976E+00-5.213788603794E-09
+     1.800074980348E-10 5.170000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 2.328306436539E-10
+     3.870650000000E+05
+E01 2019 12 12 11 10 00-7.507689879276E-04-7.986500349944E-12 0.000000000000E+00
+     3.000000000000E+00 2.013125000000E+02 2.449744898851E-09 7.981291655576E-01
+     9.473413228989E-06 1.731918891892E-04 9.533017873764E-06 5.440622966766E+03
+     3.858000000000E+05 3.166496753693E-08-2.605880888987E+00-1.117587089539E-08
+     9.855193908613E-01 1.463750000000E+02-1.069210219000E+00-5.272719629937E-09
+     1.914365455291E-10 2.580000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00-1.629814505577E-09 0.000000000000E+00
+     3.865400000000E+05
+E01 2019 12 12 11 20 00-7.507736445405E-04-7.972289495228E-12 0.000000000000E+00
+     4.000000000000E+00 2.002500000000E+02 2.449744898851E-09 8.736282654153E-01
+     9.419396519661E-06 1.731686061248E-04 9.588897228241E-06 5.440622581482E+03
+     3.864000000000E+05 3.725290298462E-08-2.605884063520E+00-1.862645149231E-09
+     9.855195488564E-01 1.452500000000E+02-1.070324887968E+00-5.265933632987E-09
+     1.896507568581E-10 2.580000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00-1.629814505577E-09 0.000000000000E+00
+     3.871400000000E+05
+E12 2019 12 12 10 40 00 6.055364909116E-03-1.871569565992E-11 0.000000000000E+00
+     0.000000000000E+00-2.075312500000E+02 2.603322724555E-09-2.414195903395E+00
+    -9.659677743912E-06 2.996901748702E-04 6.495043635368E-06 5.440614477158E+03
+     3.840000000000E+05-2.421438694000E-08-5.219517721978E-01 5.774199962616E-08
+     9.864495053986E-01 2.151250000000E+02-9.348391204646E-01-5.376652530588E-09
+    -2.257236880119E-10 2.580000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00-1.327134668827E-08 0.000000000000E+00
+     3.858400000000E+05
+E19 2019 12 12 11 00 00-2.468528691679E-06 2.273736754432E-13 0.000000000000E+00
+     2.000000000000E+00 1.103125000000E+01 3.765871149364E-09-1.685770007887E-01
+     4.824250936508E-07 1.212444622070E-04 3.688037395477E-06 5.440600940704E+03
+     3.852000000000E+05-5.401670932770E-08 1.580615112564E+00 5.215406417847E-08
+     9.583897608719E-01 2.595937500000E+02 1.996512379669E+00-5.770954669140E-09
+     1.160762636137E-10 2.580000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00-5.820766091347E-09 0.000000000000E+00
+     3.859400000000E+05
+E19 2019 12 12 11 10 00-2.468470484018E-06 2.273736754432E-13 0.000000000000E+00
+     3.000000000000E+00 1.168750000000E+01 3.765156833895E-09-9.388288915917E-02
+     5.066394805908E-07 1.211694907397E-04 3.693625330925E-06 5.440600774765E+03
+     3.858000000000E+05-5.960464477539E-08 1.580611582543E+00 4.097819328308E-08
+     9.583898398694E-01 2.594687500000E+02 1.996204499781E+00-5.764168672191E-09
+     1.207193141583E-10 2.580000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00-5.820766091347E-09 0.000000000000E+00
+     3.865400000000E+05
+E21 2019 12 12 11 10 00-5.709161632694E-04-2.117417352565E-12 0.000000000000E+00
+     3.000000000000E+00 2.180312500000E+02 2.333311477503E-09 1.537835498027E+00
+     1.022964715958E-05 9.841390419751E-05 9.685754776001E-06 5.440636165619E+03
+     3.858000000000E+05-2.980232238770E-08-2.608584029515E+00 4.842877388000E-08
+     9.850865499746E-01 1.424062500000E+02-1.025413854454E+00-5.217717338871E-09
+     1.871506527187E-10 2.580000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 0.000000000000E+00
+     3.867900000000E+05
+E21 2019 12 12 11 20 00-5.709173856303E-04-2.131628207280E-12 0.000000000000E+00
+     4.000000000000E+00 2.213125000000E+02 2.317953694933E-09 1.607657241034E+00
+     1.038052141666E-05 9.810912888497E-05 9.816139936447E-06 5.440637243271E+03
+     3.864000000000E+05-4.097819328308E-08-2.608587130902E+00 4.470348358154E-08
+     9.850867050439E-01 1.395312500000E+02-1.020851268976E+00-5.213788603794E-09
+     1.800074980348E-10 2.580000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 0.000000000000E+00
+     3.871400000000E+05
+E04 2019 12 12 11 10 00-4.236891982146E-04-7.531752999057E-12 0.000000000000E+00
+     3.000000000000E+00 9.781250000000E+00 3.935521073107E-09-4.009111796513E-01
+     5.532056093216E-07 8.811207953840E-05 3.596767783165E-06 5.440604427338E+03
+     3.858000000000E+05 9.499490261078E-08 1.582511472789E+00 3.166496753693E-08
+     9.526713574660E-01 2.577187500000E+02 1.440283619102E+00-5.908817554540E-09
+     7.071723137082E-11 5.170000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00-5.355104804039E-09-5.820766091347E-09
+     3.875670000000E+05
+E04 2019 12 12 11 10 00-4.236878594384E-04-7.531752999057E-12 0.000000000000E+00
+     3.000000000000E+00 9.781250000000E+00 3.935521073107E-09-4.009111796513E-01
+     5.532056093216E-07 8.811207953840E-05 3.596767783165E-06 5.440604427338E+03
+     3.858000000000E+05 9.499490261078E-08 1.582511472789E+00 3.166496753693E-08
+     9.526713574660E-01 2.577187500000E+02 1.440283619102E+00-5.908817554540E-09
+     7.071723137082E-11 2.580000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00-5.355104804039E-09 0.000000000000E+00
+     3.875900000000E+05
+E11 2019 12 12 11 00 00 3.295102505945E-04 1.526245796413E-10 5.204170427930E-18
+     2.000000000000E+00-2.123750000000E+02 2.665468170305E-09 2.480654590844E+00
+    -9.858980774879E-06 1.651302445680E-04 7.150694727898E-06 5.440609029770E+03
+     3.852000000000E+05-3.352761268616E-08-5.219456059981E-01 2.048909664154E-08
+     9.864349859366E-01 1.975937500000E+02-2.012781103955E-01-5.400939256513E-09
+    -2.103659054415E-10 5.170000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00-1.583248376846E-08-1.699663698673E-08
+     3.876330000000E+05
+E21 2019 12 12 11 30 00-5.709196557291E-04-2.131628207280E-12 0.000000000000E+00
+     5.000000000000E+00 2.240625000000E+02 2.303667385565E-09 1.677767938158E+00
+     1.050904393196E-05 9.777839295566E-05 9.970739483833E-06 5.440638303757E+03
+     3.870000000000E+05-5.401670932770E-08-2.608590226436E+00 3.725290298462E-08
+     9.850868571874E-01 1.362812500000E+02-1.016577701982E+00-5.210574184187E-09
+     1.721500278825E-10 5.170000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 2.328306436539E-10
+     3.876650000000E+05
+E11 2019 12 12 11 00 00 3.295124042779E-04 1.526387904960E-10 5.204170427930E-18
+     2.000000000000E+00-2.123750000000E+02 2.665468170305E-09 2.480654590844E+00
+    -9.858980774879E-06 1.651302445680E-04 7.150694727898E-06 5.440609029770E+03
+     3.852000000000E+05-3.352761268616E-08-5.219456059981E-01 2.048909664154E-08
+     9.864349859366E-01 1.975937500000E+02-2.012781103955E-01-5.400939256513E-09
+    -2.103659054415E-10 2.580000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00-1.583248376846E-08 0.000000000000E+00
+     3.876800000000E+05
+E01 2019 12 12 11 30 00-7.507792906836E-04-7.972289495228E-12 0.000000000000E+00
+     5.000000000000E+00 1.994687500000E+02 2.447601952446E-09 9.486982138657E-01
+     9.378418326378E-06 1.731649972498E-04 9.616836905479E-06 5.440622346878E+03
+     3.870000000000E+05 3.911554813385E-08-2.605887229274E+00 7.450580596924E-09
+     9.855197039257E-01 1.446875000000E+02-1.071010335308E+00-5.259504793772E-09
+     1.871506527187E-10 5.170000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00-1.629814505577E-09-1.862645149231E-09
+     3.877250000000E+05
+E21 2019 12 12 11 30 00-5.709186661988E-04-2.117417352565E-12 0.000000000000E+00
+     5.000000000000E+00 2.240625000000E+02 2.303667385565E-09 1.677767938158E+00
+     1.050904393196E-05 9.777839295566E-05 9.970739483833E-06 5.440638303757E+03
+     3.870000000000E+05-5.401670932770E-08-2.608590226436E+00 3.725290298462E-08
+     9.850868571874E-01 1.362812500000E+02-1.016577701982E+00-5.210574184187E-09
+     1.721500278825E-10 2.580000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 0.000000000000E+00
+     3.877400000000E+05
+E01 2019 12 12 11 30 00-7.507783011533E-04-7.972289495228E-12 0.000000000000E+00
+     5.000000000000E+00 1.994687500000E+02 2.447601952446E-09 9.486982138657E-01
+     9.378418326378E-06 1.731649972498E-04 9.616836905479E-06 5.440622346878E+03
+     3.870000000000E+05 3.911554813385E-08-2.605887229274E+00 7.450580596924E-09
+     9.855197039257E-01 1.446875000000E+02-1.071010335308E+00-5.259504793772E-09
+     1.871506527187E-10 2.580000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00-1.629814505577E-09 0.000000000000E+00
+     3.878200000000E+05
+E36 2019 12 12 10 30 00 6.253067986108E-04-4.803268893738E-12 0.000000000000E+00
+     1.270000000000E+02-2.396562500000E+02 2.521890761159E-09 3.060908164528E+00
+    -1.121312379837E-05 3.076605498791E-04 7.288530468941E-06 5.440609262466E+03
+     3.834000000000E+05 2.980232238770E-08-5.264936383676E-01 5.587935447693E-08
+     9.896539850266E-01 1.944062500000E+02-1.782833699345E+00-5.428440402046E-09
+    -2.457245211269E-10 5.170000000000E+02 2.083000000000E+03
+     3.120000000000E+00 0.000000000000E+00 3.725290298462E-09 4.423782229424E-09
+     3.878450000000E+05
+S36 2019 12 12 11 36 48 0.000000000000E+00 0.000000000000E+00 3.874820000000E+05
+     4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+     3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+     0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 2.530000000000E+02
+S25 2019 12 11 23 59 44 0.000000000000E+00 0.000000000000E+00 3.874840000000E+05
+     4.053076976000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+    -1.162201120000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+     0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00
+S23 2019 12 12 11 37 04 0.000000000000E+00 0.000000000000E+00 3.874940000000E+05
+     3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+     2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+     0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.460000000000E+02
+S36 2019 12 12 11 38 56 0.000000000000E+00 0.000000000000E+00 3.875460000000E+05
+     4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+     3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+     0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 2.540000000000E+02
+S23 2019 12 12 11 39 12 0.000000000000E+00 0.000000000000E+00 3.875580000000E+05
+     3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+     2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+     0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.470000000000E+02
+S23 2019 12 12 11 41 20 0.000000000000E+00 0.000000000000E+00 3.876860000000E+05
+     3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+     2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+     0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.480000000000E+02
+S36 2019 12 12 11 41 04 0.000000000000E+00 0.000000000000E+00 3.877380000000E+05
+     4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+     3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+     0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 2.550000000000E+02
+S36 2019 12 12 11 43 12 0.000000000000E+00 0.000000000000E+00 3.878020000000E+05
+     4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+     3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+     0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00
+S23 2019 12 12 11 43 28 0.000000000000E+00 0.000000000000E+00 3.878780000000E+05
+     3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+     2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+     0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.490000000000E+02
+C05 2019 12 12 11 00 00-2.244751667604E-04-5.808153957787E-11 0.000000000000E+00
+     1.000000000000E+00-4.986718750000E+02 1.785788670980E-12-5.169766818568E-01
+    -1.630326732993E-05 6.773804780096E-04 2.061109989882E-05 6.493346771240E+03
+     3.852000000000E+05 1.271255314350E-07-1.766797704845E-01 3.259629011154E-09
+     1.180676987021E-01-6.241562500000E+02-1.608095995036E+00 1.192549674481E-09
+     7.575315542299E-10 0.000000000000E+00 7.270000000000E+02
+     2.000000000000E+00 0.000000000000E+00-8.000000000000E-10-9.200000000000E-09
+     3.858276000000E+05 0.000000000000E+00
+C10 2019 12 12 11 00 00-4.537283675745E-04-3.685762806072E-11 0.000000000000E+00
+     1.000000000000E+00 9.611718750000E+02 9.743262988869E-10-3.040647920928E+00
+     3.135204315186E-05 6.156359100714E-03 7.578637450933E-06 6.493785018921E+03
+     3.852000000000E+05-1.676380634308E-08-2.379959881122E+00-4.004687070847E-08
+     9.058375010021E-01-1.493750000000E+01-2.570309406835E+00-2.115088101909E-09
+     4.782342060886E-10 0.000000000000E+00 7.270000000000E+02
+     2.000000000000E+00 0.000000000000E+00 5.000000000000E-09 2.400000000000E-09
+     3.858180000000E+05 0.000000000000E+00
+C23 2019 12 12 11 00 00-8.696540025994E-04 2.136069099379E-12 0.000000000000E+00
+     1.000000000000E+00 1.034062500000E+02 3.498002848716E-09 1.631067082891E+00
+     5.179084837437E-06 2.014992060140E-04 1.188227906823E-05 5.282628129959E+03
+     3.852000000000E+05 3.725290298462E-08-3.016985033742E+00 6.565824151039E-08
+     9.546959600159E-01 1.175156250000E+02-1.126531243634E+00-6.488127399406E-09
+    -1.864363372504E-10 0.000000000000E+00 7.270000000000E+02
+     2.000000000000E+00 0.000000000000E+00 2.430000000000E-08 2.430000000000E-08
+     3.858180000000E+05 1.000000000000E+00
+C30 2019 12 12 11 00 00 2.388220746070E-04 6.104450278599E-12 0.000000000000E+00
+     1.000000000000E+00-1.393437500000E+02 3.927663602954E-09-2.732742750025E+00
+    -6.837770342827E-06 3.649030113593E-04 1.750886440277E-06 5.282614929199E+03
+     3.852000000000E+05-3.864988684654E-08-9.521190897989E-01-5.587935447693E-09
+     9.644793109832E-01 3.275781250000E+02-4.157902561103E-01-7.082080711374E-09
+    -2.339383158984E-10 0.000000000000E+00 7.270000000000E+02
+     2.000000000000E+00 0.000000000000E+00-1.040000000000E-08-1.040000000000E-08
+     3.858180000000E+05 1.000000000000E+00
+C32 2019 12 12 11 00 00-8.566775359213E-04 2.992273095970E-12 0.000000000000E+00
+     1.000000000000E+00 2.875000000000E+01 4.228033257413E-09 2.867304095205E+00
+     1.456588506699E-06 1.842749770731E-04 6.427057087421E-06 5.282614164352E+03
+     3.852000000000E+05 1.862645149231E-09 1.148173467908E+00 6.705522537231E-08
+     9.608588315814E-01 2.297500000000E+02-1.050918258529E+00-6.857785654299E-09
+     5.228789228631E-10 0.000000000000E+00 7.270000000000E+02
+     2.000000000000E+00 0.000000000000E+00-9.900000000000E-09-9.900000000000E-09
+     3.857580000000E+05 1.000000000000E+00
+C37 2019 12 12 11 00 00-8.880557725206E-04-5.954881032721E-11 0.000000000000E+00
+     1.000000000000E+00 1.086562500000E+02 3.521932416908E-09 8.066563687499E-01
+     5.388632416725E-06 7.037379546091E-04 1.127412542701E-05 5.282630052567E+03
+     3.852000000000E+05-2.142041921616E-08-3.020306267399E+00 1.536682248116E-08
+     9.560827215058E-01 1.306718750000E+02-1.080448983277E+00-6.580988410297E-09
+    -2.328668426958E-10 0.000000000000E+00 7.270000000000E+02
+     2.000000000000E+00 0.000000000000E+00-1.390000000000E-08-1.390000000000E-08
+     3.858180000000E+05 1.000000000000E+00
+C20 2019 12 12 11 00 00-4.067538538948E-04-5.008171655163E-11 0.000000000000E+00
+     1.000000000000E+00 2.850000000000E+01 4.229461888350E-09 1.463145840983E+00
+     1.523178070784E-06 5.460308166221E-04 6.177462637424E-06 5.282614208221E+03
+     3.852000000000E+05 3.445893526077E-08 1.153432718382E+00-1.210719347000E-08
+     9.622213291121E-01 2.341406250000E+02-4.480160490039E-01-6.852785446020E-09
+     4.753769442150E-10 0.000000000000E+00 7.270000000000E+02
+     2.000000000000E+00 0.000000000000E+00 2.090000000000E-08 2.090000000000E-08
+     3.875880000000E+05 1.000000000000E+00
diff --git a/src/examples/sample_data.obs b/src/examples/sample_data.obs
new file mode 100644
index 0000000000000000000000000000000000000000..d773d3b4d57556afcd38b64ef4167981a00de992
--- /dev/null
+++ b/src/examples/sample_data.obs
@@ -0,0 +1,58 @@
+     3.04           OBSERVATION DATA    M                   RINEX VERSION / TYPE
+sbf2rin-13.4.3                          20200205 113408 UTC PGM / RUN BY / DATE 
+SEPT                                                        MARKER NAME         
+Unknown                                                     MARKER NUMBER       
+Unknown             Unknown                                 OBSERVER / AGENCY   
+3021420             SEPT ASTERX-M2      4.4.0               REC # / TYPE / VERS 
+Unknown             Unknown                                 ANT # / TYPE        
+  4789398.3686   176958.8129  4194502.0999                  APPROX POSITION XYZ 
+        0.0000        0.0000        0.0000                  ANTENNA: DELTA H/E/N
+G    7 X1  C1C L1C C2W L2W C2L L2L                          SYS / # / OBS TYPES 
+E    7 X1  C1C L1C C5Q L5Q C7Q L7Q                          SYS / # / OBS TYPES 
+S    3 X1  C1C L1C                                          SYS / # / OBS TYPES 
+R    5 X1  C1C L1C C2C L2C                                  SYS / # / OBS TYPES 
+C    5 X1  C2I L2I C7I L7I                                  SYS / # / OBS TYPES 
+SEPTENTRIO RECEIVERS OUTPUT ALIGNED CARRIER PHASES.         COMMENT             
+NO FURTHER PHASE SHIFT APPLIED IN THE RINEX ENCODER.        COMMENT             
+G L1C                                                       SYS / PHASE SHIFT   
+G L2W                                                       SYS / PHASE SHIFT   
+G L2L  0.00000                                              SYS / PHASE SHIFT   
+E L1C  0.00000                                              SYS / PHASE SHIFT   
+E L5Q  0.00000                                              SYS / PHASE SHIFT   
+E L7Q  0.00000                                              SYS / PHASE SHIFT   
+S L1C                                                       SYS / PHASE SHIFT   
+R L1C                                                       SYS / PHASE SHIFT   
+R L2C                                                       SYS / PHASE SHIFT   
+C L2I                                                       SYS / PHASE SHIFT   
+C L7I                                                       SYS / PHASE SHIFT   
+     0.050                                                  INTERVAL            
+  2019    12    12    11    37   42.0000000     GPS         TIME OF FIRST OBS   
+  2019    12    12    11    45   12.9000000     GPS         TIME OF LAST OBS    
+    36                                                      # OF SATELLITES     
+ C1C    0.000 C2C    0.000                                  GLONASS COD/PHS/BIS 
+  8 R03  5 R05  1 R13 -2 R14 -7 R15  0 R19  3 R20  2 R21  4 GLONASS SLOT / FRQ #
+                                                            END OF HEADER       
+> 2019 12 12 11 37 42.0000000  0 23
+C05        16.000    40138840.659 6 209013478.40606  40138832.256 6 161622441.87406
+C10        17.000    39633104.355 5 206379985.90205  39633100.852 6 159586077.69206
+C23        18.000    22865676.301 8 119067585.90908
+C32        19.000    22153454.599 8 115358921.19508
+C37        20.000    23690566.611 7 123363010.77907
+E01        11.000    26311074.676 7 138265636.53007  26311074.470 5 103250320.19005  26311070.977 7 105943790.57907
+E09        12.000    25723493.482 5
+E12        13.000    24803095.627 5 130341141.55605  24803092.768 4  97332661.71504  24803092.407 6  99871767.93506
+E19        14.000    24540665.652 6 128962068.93606  24540664.837 5  96302837.53105  24540661.694 6  98815075.63606
+E21        15.000    25609802.251 7 134580416.73507  25609802.381 7 100498367.38007  25609799.519 7 103120055.01507
+G02         1.000    22032430.127 5
+G06         2.000    22147435.410 6 116385570.45206  22147437.095 4  90690045.24704  22147437.422 7  90690046.16907
+G07         3.000    20902411.481 8 109842911.60108  20902406.002 6  85591857.15006  20902406.930 7  85591857.16507
+G09         4.000    21908403.525 6 115129430.37406  21908399.544 1  89711233.32401  21908399.320 5
+G23         5.000    24104714.314 6 126671129.49306  24104706.816 2  98704739.67602
+G30         6.000    21404145.095 8 112479542.93308  21404142.953 6  87646388.26706  21404143.916 7  87646383.27307
+R03         7.000    21945110.617 7 117474090.63807  21945109.720 6  91368580.68606
+R14         8.000    20214975.526 8 107757316.39008  20214976.579 7  83811176.41707
+R15         9.000    22699008.378 5 121296675.96005  22699009.712 5  94341922.34705
+R19        10.000    22853592.745 4                  22853590.888 4
+S23        21.000    38309228.895 7 201316353.13407
+S25        22.000    37834172.957 6 198818604.39206
+S36        23.000    37630702.258 7 197750698.01307
diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp
index dcc3f24cc5aa51cb925e9211b4b610f9fe4b1502..70bad327efe878de41efd6fd2c21d5f7f4e51528 100644
--- a/src/gnss_utils.cpp
+++ b/src/gnss_utils.cpp
@@ -1,64 +1,510 @@
 #include "gnss_utils/gnss_utils.h"
 
-
 namespace GNSSUtils
 {
-  
-  ComputePosOutput computePos(const GNSSUtils::Observations & _observations,
-                                    GNSSUtils::Navigation & _navigation,
-                                    const prcopt_t & _prcopt)
-  {
+ComputePosOutput computePos(const GNSSUtils::Observations& _observations,
+                            GNSSUtils::Navigation&         _navigation,
+                            const prcopt_t&                _prcopt)
+{
+  // Remove duplicated satellites
+  _navigation.uniqueNavigation();
 
-    // Remove duplicated satellites
-    uniqnav(&(_navigation.getNavigation()));
+  // Define error msg
+  char msg[128] = "";
 
-    // Define error msg
-    char msg[128] = "";
+  // Remove duplicated satellites
+  _navigation.uniqueNavigation();
 
-    GNSSUtils::ComputePosOutput output;
-    sol_t sol;
-    sol = {{0}};
+  GNSSUtils::ComputePosOutput output;
+  sol_t                       sol;
+  sol = { { 0 } };
 
-    output.pos_stat = pntpos(_observations.getObservations().data(), _observations.getObservations().size(),
+    output.pos_stat = pntpos(_observations.data(), _observations.size(),
                             &(_navigation.getNavigation()),
                             &_prcopt, &sol, NULL, NULL, msg);
     
     if (output.pos_stat == 0)
     {
-      std::cout << "Bad computing: "  << msg << "\n";
+      std::cout << "computePos: error in computing positioning, message: "  << msg << "\n";
     }
 
     output.time = sol.time.time;
-    output.time = sol.time.sec;
+    output.sec = sol.time.sec;
     output.pos  = Eigen::Vector3d(sol.rr);
-    // std::cout << "Compute pos:  " << output.pos.transpose() << "\n";
     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[3], sol.qr[2];
+                        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 = ecefToLatLon(output.pos);
+    output.lat_lon = ecefToLatLonAlt(output.pos);
 
     return output;
-  }
+}
 
-  Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef)
-  {
-    double pos[3];
-    ecef2pos(&_ecef(0), pos);
+// ComputePosOutput computePosOwn(const GNSSUtils::Observations & _observations,
+//                                      GNSSUtils::Navigation & _navigation,
+//                                      const prcopt_t & _prcopt)
+//   {
+
+//   // Remove duplicated satellites
+//   uniqnav(&(_navigation.getNavigation()));
+
+//   // Define error msg
+//   char msg[128] = "";
+
+//   GNSSUtils::ComputePosOutput output;
+//   sol_t sol;
+//   sol = {{0}};
+
+//   output.pos_stat = pntposOwn(_observations.data(), _observations.size(),
+//                             &(_navigation.getNavigation()),
+//                             &_prcopt, &sol, NULL, NULL, msg);
+
+//   if (output.pos_stat == 0)
+//   {
+//   std::cout << "Bad computing: "  << msg << "\n";
+//   }
+
+//   output.time = sol.time.time;
+//   output.sec = sol.time.sec;
+//   output.pos  = Eigen::Vector3d(sol.rr);
+//   // std::cout << "Compute pos:  " << output.pos.transpose() << "\n";
+//   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[3], sol.qr[2];
+
+//   // 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;
+//   }
+
+/* single-point positioning COPIED FROM RTKLIB ------------------------------
+ * compute receiver position, velocity, clock bias by single-point positioning
+ * with pseudorange and doppler observables
+ * args   : obsd_t *obs      I   observation data
+ *          int    n         I   number of observation data
+ *          nav_t  *nav      I   navigation data
+ *          prcopt_t *opt    I   processing options
+ *          sol_t  *sol      IO  solution
+ *          double *azel     IO  azimuth/elevation angle (rad) (NULL: no output)
+ *          ssat_t *ssat     IO  satellite status              (NULL: no output)
+ *          char   *msg      O   error message for error exit
+ * return : status(1:ok,0:error)
+ * notes  : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and
+ *          receiver bias are negligible (only involving glonass-gps time offset
+ *          and receiver bias)
+ *-----------------------------------------------------------------------------*/
+// int pntposOwn(const obsd_t *obs, int n, const nav_t *nav,
+//               const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
+//               char *msg)
+// {
+//     prcopt_t opt_=*opt;
+//     double *rs,*dts,*var,*azel_,*resp;
+//     int i,stat,vsat[MAXOBS]={0},svh[MAXOBS];
+
+//     sol->stat=SOLQ_NONE;
+
+//     if (n<=0) {strcpy(msg,"no observation data"); return 0;}
+
+//     trace(3,"pntpos  : tobs=%s n=%d\n",time_str(obs[0].time,3),n);
+
+//     sol->time=obs[0].time; msg[0]='\0';
+
+//     rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n);
+
+//     if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */
+// #if 0
+//         opt_.sateph =EPHOPT_BRDC;
+// #endif
+//         opt_.ionoopt=IONOOPT_BRDC;
+//         opt_.tropopt=TROPOPT_SAAS;
+//     }
+//     /* satellite positons, velocities and clocks */
+//     satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh);
+
+//     /* estimate receiver position with pseudorange */
+//     stat=estposOwn(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
+
+//     /* raim fde */
+//     if (!stat&&n>=6&&opt->posopt[4]) {
+//         stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
+//     }
+//     /* estimate receiver velocity with doppler */
+//     //if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat);
+
+//     if (azel) {
+//         for (i=0;i<n*2;i++) azel[i]=azel_[i];
+//     }
+//     if (ssat) {
+//         for (i=0;i<MAXSAT;i++) {
+//             ssat[i].vs=0;
+//             ssat[i].azel[0]=ssat[i].azel[1]=0.0;
+//             ssat[i].resp[0]=ssat[i].resc[0]=0.0;
+//             ssat[i].snr[0]=0;
+//         }
+//         for (i=0;i<n;i++) {
+//             ssat[obs[i].sat-1].azel[0]=azel_[  i*2];
+//             ssat[obs[i].sat-1].azel[1]=azel_[1+i*2];
+//             ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0];
+//             if (!vsat[i]) continue;
+//             ssat[obs[i].sat-1].vs=1;
+//             ssat[obs[i].sat-1].resp[0]=resp[i];
+//         }
+//     }
+//     free(rs); free(dts); free(var); free(azel_); free(resp);
+//     return stat;
+// }
+
+int estposOwn(const obsd_t*   obs,
+              int             n,
+              const double*   rs,
+              const double*   dts,
+              const double*   vare,
+              const int*      svh,
+              const nav_t*    nav,
+              const prcopt_t* opt,
+              sol_t*          sol,
+              double*         azel,
+              int*            vsat,
+              double*         resp,
+              char*           msg)
+{
+  //      double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig;
+  //      int i,j,k,info,stat,nv,ns;
+  //
+  //      trace(3,"estpos  : n=%d\n",n);
+  //
+  //      v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1);
+  //
+  //      for (i=0;i<3;i++) x[i]=sol->rr[i];
+  //
+  //      for (i=0;i<MAXITR;i++) {
+  //
+  //          /* pseudorange residuals */
+  //          nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
+  //                     &ns);
+  //
+  //          if (nv<NX) {
+  //              sprintf(msg,"lack of valid sats ns=%d",nv);
+  //              break;
+  //          }
+  //          /* weight by variance */
+  //          for (j=0;j<nv;j++) {
+  //              sig=sqrt(var[j]);
+  //              v[j]/=sig;
+  //              for (k=0;k<NX;k++) H[k+j*NX]/=sig;
+  //          }
+  //          /* least square estimation */
+  //          if ((info=lsq(H,v,NX,nv,dx,Q))) {
+  //              sprintf(msg,"lsq error info=%d",info);
+  //              break;
+  //          }
+  //          for (j=0;j<NX;j++) x[j]+=dx[j];
+  //
+  //          if (norm(dx,NX)<1E-4) {
+  //              sol->type=0;
+  //              sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
+  //              sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
+  //              sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
+  //              sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
+  //              sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
+  //              for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
+  //              for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
+  //              sol->qr[3]=(float)Q[1];    /* cov xy */
+  //              sol->qr[4]=(float)Q[2+NX]; /* cov yz */
+  //              sol->qr[5]=(float)Q[2];    /* cov zx */
+  //              sol->ns=(unsigned char)ns;
+  //              sol->age=sol->ratio=0.0;
+  //
+  //              /* validate solution */
+  //              if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) {
+  //                  sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE;
+  //              }
+  //              free(v); free(H); free(var);
+  //
+  //              return stat;
+  //          }
+  //      }
+  //      if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);
+  //
+  //      free(v); free(H); free(var);
+
+  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;
+}
 
-    return Eigen::Vector3d(pos);
+double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef)
+{
+  // ecef 2 geodetic
+  Eigen::Vector3d receiver_geo;
+  ecef2pos(receiver_ecef.data(), receiver_geo.data());
+
+  // receiver-sat vector ecef
+  Eigen::Vector3d receiver_sat_ecef = sat_ecef - receiver_ecef;
+
+  // receiver-sat vector enu (receiver ecef as origin)
+  Eigen::Vector3d receiver_sat_enu;
+  ecef2enu(receiver_geo.data(),       // geodetic position {lat,lon} (rad)
+           receiver_sat_ecef.data(),  // vector in ecef coordinate {x,y,z}
+           receiver_sat_enu.data());  // vector in local tangental coordinate {e,n,u}
+
+  // elevation
+  return atan2(receiver_sat_enu(2),
+               sqrt(receiver_sat_enu(0) * receiver_sat_enu(0) + receiver_sat_enu(1) * receiver_sat_enu(1)));
+}
+
+void computeSatellitesPositions(const Observations&             obs,
+                                const Navigation&               nav,
+                                const prcopt_t&                 opt,
+                                std::map<int, Eigen::Vector3d>& sats_pos)
+{
+  double rs[6 * obs.size()], dts[2 * obs.size()], var[obs.size()];
+  int    svh[obs.size()];
+
+  // std::cout << "computing sats position from sats: ";
+  // for (auto&& obs_ref : obs.getObservations())
+  //    std::cout << (int)obs_ref.sat << " ";
+  // std::cout << std::endl;
+
+  // compute positions
+  satposs(
+      obs.getObservations().front().time, obs.data(), obs.size(), &nav.getNavigation(), opt.sateph, rs, dts, var, svh);
+
+  // store positions
+  // std::cout << "filling sats positions: \n";
+  for (int i = 0; i < obs.size(); i++)
+  {
+    if (svh[i] < 0)  // ephemeris unavailable
+      sats_pos[obs.getObservationByIdx(i).sat] = Eigen::Vector3d::Zero();
+    else
+      sats_pos[obs.getObservationByIdx(i).sat] << rs[6 * i], rs[6 * i + 1], rs[6 * i + 2];
+    // std::cout << "\tsat: " << (int)obs.getObservationByIdx(i).sat << ": " <<
+    // sats_pos[obs.getObservationByIdx(i).sat].transpose() << std::endl;
   }
+}
+
+bool equalTime(const gtime_t& time1, const gtime_t& time2)
+{
+  return (difftime(time1.time, time2.time) == 0.0 && time1.sec == time2.sec);
+}
+
+bool equalObservations(const obsd_t& obs1, const obsd_t& obs2)
+{
+  if (!equalTime(obs1.time, obs2.time))
+    return false;
+  if (!equalTime(obs1.eventime, obs2.eventime))
+    return false;
+  if (obs1.timevalid != obs2.timevalid)
+    return false;
+  if (obs1.sat != obs2.sat)
+    return false;
+  if (obs1.rcv != obs2.rcv)
+    return false;
+  if (memcmp(obs1.SNR, obs2.SNR, sizeof(obs1.SNR)) != 0)
+    return false;
+  if (memcmp(obs1.LLI, obs2.LLI, sizeof(obs1.LLI)) != 0)
+    return false;
+  if (memcmp(obs1.code, obs2.code, sizeof(obs1.code)) != 0)
+    return false;
+  if (memcmp(obs1.qualL, obs2.qualL, sizeof(obs1.qualL)) != 0)
+    return false;
+  if (memcmp(obs1.qualP, obs2.qualP, sizeof(obs1.qualP)) != 0)
+    return false;
+  if (obs1.freq != obs2.freq)
+    return false;
+  if (!equalArray<double>(obs1.L, obs2.L, sizeof(obs1.L) / sizeof(obs1.L[0]), sizeof(obs2.L) / sizeof(obs2.L[0])))
+    return false;
+  if (!equalArray<double>(obs1.P, obs2.P, sizeof(obs1.P) / sizeof(obs1.P[0]), sizeof(obs2.P) / sizeof(obs2.P[0])))
+    return false;
+  if (!equalArray<float>(obs1.D, obs2.D, sizeof(obs1.D) / sizeof(obs1.D[0]), sizeof(obs2.D) / sizeof(obs2.D[0])))
+    return false;
 
+  return true;
 }
+
+bool equalObservations(const obs_t& obs1, const obs_t& obs2)
+{
+  if (obs1.n != obs2.n)
+    return false;
+  if (obs1.nmax != obs2.nmax)
+    return false;
+  if (obs1.flag != obs2.flag)
+    return false;
+  if (obs1.rcvcount != obs2.rcvcount)
+    return false;
+  if (obs1.tmcount != obs2.tmcount)
+    return false;
+  if (!equalObservations(*(obs1.data), *(obs2.data)))
+    return false;
+
+  return true;
+}
+
+}  // namespace GNSSUtils
diff --git a/src/navigation.cpp b/src/navigation.cpp
index d53cf51287ae59572b4885aa0fba47cf0420dbe4..398d0a86f275b64f4d469a2bce0ddbdf3dba6172 100644
--- a/src/navigation.cpp
+++ b/src/navigation.cpp
@@ -1,96 +1,265 @@
 #include "gnss_utils/navigation.h"
 
-
 using namespace GNSSUtils;
 
-
 Navigation::Navigation()
 {
-    allocateEphemeris(0);
-    allocateGLONASSEphemeris(0);
-    allocateSBASEphemeris(0);
-    allocateAlmanac(0);
+  // array initialization
+  nav_.n = nav_.nmax = nav_.ng = nav_.ngmax = nav_.ns = nav_.nsmax = nav_.ne = nav_.nemax = nav_.nc = nav_.ncmax =
+      nav_.na = nav_.namax = nav_.nt = nav_.ntmax = nav_.nf = nav_.nfmax = 0;
+  nav_.eph                                                               = NULL;
+  nav_.geph                                                              = NULL;
+  nav_.seph                                                              = NULL;
+  nav_.peph                                                              = NULL;
+  nav_.pclk                                                              = NULL;
+  nav_.alm                                                               = NULL;
+  nav_.tec                                                               = NULL;
+  nav_.fcb                                                               = NULL;
+  clearNavigation();
 }
 
-Navigation::~Navigation()
+Navigation::Navigation(const Navigation& nav)
 {
+  // array initialization
+  nav_.n = nav_.nmax = nav_.ng = nav_.ngmax = nav_.ns = nav_.nsmax = nav_.ne = nav_.nemax = nav_.nc = nav_.ncmax =
+      nav_.na = nav_.namax = nav_.nt = nav_.ntmax = nav_.nf = nav_.nfmax = 0;
+  nav_.eph                                                               = NULL;
+  nav_.geph                                                              = NULL;
+  nav_.seph                                                              = NULL;
+  nav_.peph                                                              = NULL;
+  nav_.pclk                                                              = NULL;
+  nav_.alm                                                               = NULL;
+  nav_.tec                                                               = NULL;
+  nav_.fcb                                                               = NULL;
   clearNavigation();
-  //free(_nav);
+  setNavigation(nav.getNavigation());
 }
 
-void Navigation::clearNavigation()
+Navigation::~Navigation()
 {
-  deleteEphemeris();
-  deleteGLONASSEphemeris();
-  deleteSBASEphemeris();
-  deleteAlmanac();
+  freeNavigationArrays();
 }
 
 void Navigation::setNavigation(nav_t _nav)
 {
-    clearNavigation();
-    nav_ = _nav;
-}
+  freeNavigationArrays();
 
-const nav_t & Navigation::getNavigation() const
-{
-  return nav_;
-}
+  copyAllArrays(_nav);
+  //    int n,nmax;         /* number of broadcast ephemeris */
+  //    int ng,ngmax;       /* number of glonass ephemeris */
+  //    int ns,nsmax;       /* number of sbas ephemeris */
+  //    int ne,nemax;       /* number of precise ephemeris */
+  //    int nc,ncmax;       /* number of precise clock */
+  //    int na,namax;       /* number of almanac data */
+  //    int nt,ntmax;       /* number of tec grid data */
+  //    int nf,nfmax;       /* number of satellite fcb data */
+  //    eph_t *eph;         /* GPS/QZS/GAL ephemeris */
+  //    geph_t *geph;       /* GLONASS ephemeris */
+  //    seph_t *seph;       /* SBAS ephemeris */
+  //    peph_t *peph;       /* precise ephemeris */
+  //    pclk_t *pclk;       /* precise clock */
+  //    alm_t *alm;         /* almanac data */
+  //    tec_t *tec;         /* tec grid data */
+  //    fcbd_t *fcb;        /* satellite fcb data */
+  //    double lam[MAXSAT][NFREQ]; /* carrier wave lengths (m) */
 
-nav_t & Navigation::getNavigation()
-{
-  return nav_;
-}
+  copyIonUtc(_nav);
+  //    double utc_gps[4];  /* GPS delta-UTC parameters {A0,A1,T,W} */
+  //    double utc_glo[4];  /* GLONASS UTC GPS time parameters */
+  //    double utc_gal[4];  /* Galileo UTC GPS time parameters */
+  //    double utc_qzs[4];  /* QZS UTC GPS time parameters */
+  //    double utc_cmp[4];  /* BeiDou UTC parameters */
+  //    double utc_irn[4];  /* IRNSS UTC parameters */
+  //    double utc_sbs[4];  /* SBAS UTC parameters */
+  //    double ion_gps[8];  /* GPS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
+  //    double ion_gal[4];  /* Galileo iono model parameters {ai0,ai1,ai2,0} */
+  //    double ion_qzs[8];  /* QZSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
+  //    double ion_cmp[8];  /* BeiDou iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
+  //    double ion_irn[8];  /* IRNSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
+  //    int leaps;          /* leap seconds (s) */
 
+  copySbasCorrections(_nav);
+  //    sbssat_t sbssat;    /* SBAS satellite corrections */
+  //    sbsion_t sbsion[MAXBAND+1]; /* SBAS ionosphere corrections */
 
-/****************** Array memory management ******************/
-void Navigation::allocateEphemeris(int n_sat)
-{
-  eph_t eph0 ={0,-1,-1};
-  int i;
-  nav_.eph = (eph_t *) malloc(sizeof(eph_t)*n_sat);
-  for (i=0;i<n_sat;i++) nav_.eph[i] = eph0;
+  // ********** other not copied nav_t content: ***********
+  //    erp_t  erp;         /* earth rotation parameters */
+  //    double cbias[MAXSAT][3]; /* satellite dcb (0:p1-p2,1:p1-c1,2:p2-c2) (m) */
+  //    double rbias[MAXRCV][2][3]; /* receiver dcb (0:p1-p2,1:p1-c1,2:p2-c2) (m) */
+  //    double wlbias[MAXSAT];   /* wide-lane bias (cycle) */
+  //    double glo_cpbias[4];    /* glonass code-phase bias {1C,1P,2C,2P} (m) */
+  // SHOULD WE COPY THIS????????  char glo_fcn[MAXPRNGLO+1]; /* glonass frequency channel number + 8 */
+  //    pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */
+  //    dgps_t dgps[MAXSAT]; /* DGPS corrections */
+  //    ssr_t ssr[MAXSAT];  /* SSR corrections */
+  //    lexeph_t lexeph[MAXSAT]; /* LEX ephemeris */
+  //    lexion_t lexion;    /* LEX ionosphere correction */
+  //    pppcorr_t pppcorr;  /* ppp corrections */
 }
-void Navigation::deleteEphemeris()
+
+void Navigation::clearNavigation()
 {
-  free(nav_.eph);
+  freeNavigationArrays();
+  memset(nav_.utc_gps, 0, sizeof(nav_.utc_gps));
+  memset(nav_.utc_glo, 0, sizeof(nav_.utc_glo));
+  memset(nav_.utc_gal, 0, sizeof(nav_.utc_gal));
+  memset(nav_.utc_qzs, 0, sizeof(nav_.utc_qzs));
+  memset(nav_.utc_cmp, 0, sizeof(nav_.utc_cmp));
+  memset(nav_.utc_irn, 0, sizeof(nav_.utc_irn));
+  memset(nav_.utc_sbs, 0, sizeof(nav_.utc_sbs));
+  memset(nav_.ion_gps, 0, sizeof(nav_.ion_gps));
+  memset(nav_.ion_gal, 0, sizeof(nav_.ion_gal));
+  memset(nav_.ion_qzs, 0, sizeof(nav_.ion_qzs));
+  memset(nav_.ion_cmp, 0, sizeof(nav_.ion_cmp));
+  memset(nav_.ion_irn, 0, sizeof(nav_.ion_irn));
+  nav_.leaps           = 0;
+  sbssat_t sbssat_zero = { 0 };
+  nav_.sbssat          = sbssat_zero;
 }
 
-void Navigation::allocateGLONASSEphemeris(int n_sat)
+void Navigation::loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt, const char* opt)
 {
-  geph_t geph0={0,-1};
-  int i;
-  nav_.geph = (geph_t *)malloc(sizeof(geph_t)*n_sat);
+  auto stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, NULL, &nav_, NULL);
+  if (stat == 1)
+  {
+    std::cout << "Navigation file loaded." << std::endl;
+    std::cout << "GPS satellites in navigation file: " << nav_.n << std::endl;
+    std::cout << "GLONASS satellites in navigation file: " << nav_.ng << std::endl;
+    std::cout << "SBAS satellites in navigation file: " << nav_.ns << std::endl;
+    std::cout << "Almanac satellites in navigation file: " << nav_.na << std::endl;
+    uniqueNavigation();
+  }
+  else
+    std::cout << "Navigation: couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error") << std::endl;
 }
-void Navigation::deleteGLONASSEphemeris()
+
+void Navigation::copyAllArrays(const nav_t& nav)
 {
-  free(nav_.geph);
+  // GPS/QZS/GAL ephemeris
+  copyArray<eph_t>(nav.eph, nav.n, nav_.eph, nav_.n, nav_.nmax);
+  // GLONASS ephemeris
+  copyArray<geph_t>(nav.geph, nav.ng, nav_.geph, nav_.ng, nav_.ngmax);
+  // SBAS ephemeris
+  copyArray<seph_t>(nav.seph, nav.ns, nav_.seph, nav_.ns, nav_.nsmax);
+  // precise ephemeris
+  copyArray<peph_t>(nav.peph, nav.ne, nav_.peph, nav_.ne, nav_.nemax);
+  // precise clock
+  copyArray<pclk_t>(nav.pclk, nav.nc, nav_.pclk, nav_.nc, nav_.ncmax);
+  // almanac data
+  copyArray<alm_t>(nav.alm, nav.na, nav_.alm, nav_.na, nav_.namax);
+  // tec grid data
+  copyArray<tec_t>(nav.tec, nav.nt, nav_.tec, nav_.nt, nav_.ntmax);
+  // satellite fcb data
+  copyArray<fcbd_t>(nav.fcb, nav.nf, nav_.fcb, nav_.nf, nav_.nfmax);
+
+  uniqueNavigation();
 }
 
-void Navigation::allocateSBASEphemeris(int n_sat)
+void Navigation::copyEphemeris(const nav_t& nav)
 {
-  seph_t seph0={0};
-  int i;
+  copyArray<eph_t>(nav.eph, nav.n, nav_.eph, nav_.n, nav_.nmax);
+  //  if (nav.eph != NULL)
+  //  {
+  //    for (int ii = 0; ii < nav.n; ++ii)
+  //    {
+  //      addEphemeris(nav.eph[ii]);
+  //    }
+  //  }
 
-  nav_.seph = (seph_t *)malloc(sizeof(seph_t)*n_sat);
-  for (i=0; i<n_sat; i++) nav_.seph[i] = seph0;
+  copyArray<geph_t>(nav.geph, nav.ng, nav_.geph, nav_.ng, nav_.ngmax);
+  //  if (nav.geph != NULL)
+  //  {
+  //    for (int ii = 0; ii < nav.ng; ++ii)
+  //    {
+  //      addGLONASSEphemeris(nav.geph[ii]);
+  //    }
+  //  }
+
+  copyArray<seph_t>(nav.seph, nav.ns, nav_.seph, nav_.ns, nav_.nsmax);
+  //  if (nav.seph != NULL)
+  //  {
+  //    for (int ii = 0; ii < nav.ns; ++ii)
+  //    {
+  //      addSBASEphemeris(nav.seph[ii]);
+  //    }
+  //  }
+
+  uniqueNavigation();
 }
-void Navigation::deleteSBASEphemeris()
+
+void Navigation::copyAlmanac(const nav_t& nav)
 {
-  free(nav_.seph);
+  copyArray<alm_t>(nav.alm, nav.na, nav_.alm, nav_.na, nav_.namax);
+  //  if (nav.alm != NULL)
+  //  {
+  //    for (int ii = 0; ii < nav.na; ++ii)
+  //    {
+  //      addAlmanac(nav.alm[ii]);
+  //    }
+  //  }
 }
 
-void Navigation::allocateAlmanac(int n_sat)
+void Navigation::copyIonUtc(const nav_t& nav)
 {
-  alm_t  alm0 ={0,-1};
-  int i;
+  std::copy(nav.utc_gps, nav.utc_gps + 4, nav_.utc_gps);
+  std::copy(nav.utc_gal, nav.utc_gal + 4, nav_.utc_gal);
+  std::copy(nav.utc_qzs, nav.utc_qzs + 4, nav_.utc_qzs);
+  std::copy(nav.utc_glo, nav.utc_glo + 4, nav_.utc_glo);
+  std::copy(nav.utc_cmp, nav.utc_cmp + 4, nav_.utc_cmp);
+  std::copy(nav.utc_irn, nav.utc_irn + 4, nav_.utc_irn);
+  std::copy(nav.utc_sbs, nav.utc_sbs + 4, nav_.utc_sbs);
 
-  nav_.alm = (alm_t *)malloc(sizeof(alm_t)*n_sat);
-  for (i=0; i<n_sat; i++) nav_.alm[i] = alm0;
+  std::copy(nav.ion_gps, nav.ion_gps + 8, nav_.ion_gps);
+  std::copy(nav.ion_gal, nav.ion_gal + 4, nav_.ion_gal);
+  std::copy(nav.ion_qzs, nav.ion_qzs + 8, nav_.ion_qzs);
+  std::copy(nav.ion_cmp, nav.ion_cmp + 4, nav_.ion_cmp);
+  std::copy(nav.ion_irn, nav.ion_irn + 8, nav_.ion_irn);
+
+  nav_.leaps = nav.leaps;
+}
+
+void Navigation::copySbasCorrections(const nav_t& nav)
+{
+  nav_.sbssat.iodp = nav.sbssat.iodp;
+  nav_.sbssat.nsat = nav.sbssat.nsat;
+  nav_.sbssat.tlat = nav.sbssat.tlat;
+  for (int i = 0; i < MAXSAT; i++)
+  {
+    nav_.sbssat.sat[i].sat        = nav.sbssat.sat[i].sat;
+    nav_.sbssat.sat[i].fcorr      = nav.sbssat.sat[i].fcorr;
+    nav_.sbssat.sat[i].lcorr.daf0 = nav.sbssat.sat[i].lcorr.daf0;
+    nav_.sbssat.sat[i].lcorr.daf1 = nav.sbssat.sat[i].lcorr.daf1;
+    nav_.sbssat.sat[i].lcorr.iode = nav.sbssat.sat[i].lcorr.iode;
+    nav_.sbssat.sat[i].lcorr.t0   = nav.sbssat.sat[i].lcorr.t0;
+    std::copy(nav.sbssat.sat[i].lcorr.dpos, nav.sbssat.sat[i].lcorr.dpos + 3, nav_.sbssat.sat[i].lcorr.dpos);
+    std::copy(nav.sbssat.sat[i].lcorr.dvel, nav.sbssat.sat[i].lcorr.dvel + 3, nav_.sbssat.sat[i].lcorr.dvel);
+  }
+  for (int i = 0; i < MAXBAND + 1; i++)
+  {
+    nav_.sbsion[i].iodi = nav.sbsion[i].iodi;
+    nav_.sbsion[i].nigp = nav.sbsion[i].nigp;
+    std::copy(nav.sbsion[i].igp, nav.sbsion[i].igp + MAXNIGP, nav_.sbsion[i].igp);
+  }
 }
-void Navigation::deleteAlmanac()
+
+void Navigation::freeNavigationArrays()
 {
-  free(nav_.alm);
+  // RTKLIB "freenav(&nav_,255)" doesn't check if is NULL before freeing
+  freeEphemeris();
+  // std::cout << "freeing glonass ephemeris...\n";
+  freeGlonassEphemeris();
+  // std::cout << "freeing sbas ephemeris...\n";
+  freeSbasEphemeris();
+  // std::cout << "freeing precise ephemeris...\n";
+  freePreciseEphemeris();
+  // std::cout << "freeing precise clock...\n";
+  freePreciseClock();
+  // std::cout << "freeing almanac...\n";
+  freeAlmanac();
+  // std::cout << "freeing tec...\n";
+  freeTecData();
+  // std::cout << "freeing fcb...\n";
+  freeFcbData();
 }
 
 void Navigation::print()
@@ -102,26 +271,33 @@ void Navigation::print()
   std::cout << "nc: " << nav_.nc << "\n";
   std::cout << "na: " << nav_.na << "\n";
   std::cout << "nt: " << nav_.nt << "\n";
-  std::cout << "nn: " << nav_.nn << "\n";
-
-  std::cout << "nmax: " << nav_.nmax << "\n";   
-  std::cout << "ngmax: " << nav_.ngmax << "\n"; 
-  std::cout << "nsmax: " << nav_.nsmax << "\n"; 
-  std::cout << "nemax: " << nav_.nemax << "\n"; 
-  std::cout << "ncmax: " << nav_.ncmax << "\n"; 
-  std::cout << "namax: " << nav_.namax << "\n"; 
-  std::cout << "ntmax: " << nav_.ntmax << "\n"; 
-  std::cout << "nnmax: " << nav_.nnmax << "\n"; 
-
-  std::cout << "utc_gps: " << nav_.utc_gps[0] << " " << nav_.utc_gps[1] << " " << nav_.utc_gps[2] << " " << nav_.utc_gps[3] << "\n";
-  std::cout << "utc_glo: " << nav_.utc_glo[0] << " " << nav_.utc_glo[1] << " " << nav_.utc_glo[2] << " " << nav_.utc_glo[3] << "\n";
-  std::cout << "utc_gal: " << nav_.utc_gal[0] << " " << nav_.utc_gal[1] << " " << nav_.utc_gal[2] << " " << nav_.utc_gal[3] << "\n";
-  std::cout << "utc_qzs: " << nav_.utc_qzs[0] << " " << nav_.utc_qzs[1] << " " << nav_.utc_qzs[2] << " " << nav_.utc_qzs[3] << "\n";
-  std::cout << "utc_cmp: " << nav_.utc_cmp[0] << " " << nav_.utc_cmp[1] << " " << nav_.utc_cmp[2] << " " << nav_.utc_cmp[3] << "\n";
-  std::cout << "utc_sbs: " << nav_.utc_sbs[0] << " " << nav_.utc_sbs[1] << " " << nav_.utc_sbs[2] << " " << nav_.utc_sbs[3] << "\n";
-  std::cout << "ion_gps: " << nav_.ion_gps[0] << " " << nav_.ion_gps[1] << " " << nav_.ion_gps[2] << " " << nav_.ion_gps[3] << "\n";
-  std::cout << "ion_gal: " << nav_.ion_gal[0] << " " << nav_.ion_gal[1] << " " << nav_.ion_gal[2] << " " << nav_.ion_gal[3] << "\n";
-  std::cout << "ion_qzs: " << nav_.ion_qzs[0] << " " << nav_.ion_qzs[1] << " " << nav_.ion_qzs[2] << " " << nav_.ion_qzs[3] << "\n";
-  std::cout << "ion_cmp: " << nav_.ion_cmp[0] << " " << nav_.ion_cmp[1] << " " << nav_.ion_cmp[2] << " " << nav_.ion_cmp[3] << "\n";
 
+  std::cout << "nmax: " << nav_.nmax << "\n";
+  std::cout << "ngmax: " << nav_.ngmax << "\n";
+  std::cout << "nsmax: " << nav_.nsmax << "\n";
+  std::cout << "nemax: " << nav_.nemax << "\n";
+  std::cout << "ncmax: " << nav_.ncmax << "\n";
+  std::cout << "namax: " << nav_.namax << "\n";
+  std::cout << "ntmax: " << nav_.ntmax << "\n";
+
+  std::cout << "utc_gps: " << nav_.utc_gps[0] << " " << nav_.utc_gps[1] << " " << nav_.utc_gps[2] << " "
+            << nav_.utc_gps[3] << "\n";
+  std::cout << "utc_glo: " << nav_.utc_glo[0] << " " << nav_.utc_glo[1] << " " << nav_.utc_glo[2] << " "
+            << nav_.utc_glo[3] << "\n";
+  std::cout << "utc_gal: " << nav_.utc_gal[0] << " " << nav_.utc_gal[1] << " " << nav_.utc_gal[2] << " "
+            << nav_.utc_gal[3] << "\n";
+  std::cout << "utc_qzs: " << nav_.utc_qzs[0] << " " << nav_.utc_qzs[1] << " " << nav_.utc_qzs[2] << " "
+            << nav_.utc_qzs[3] << "\n";
+  std::cout << "utc_cmp: " << nav_.utc_cmp[0] << " " << nav_.utc_cmp[1] << " " << nav_.utc_cmp[2] << " "
+            << nav_.utc_cmp[3] << "\n";
+  std::cout << "utc_sbs: " << nav_.utc_sbs[0] << " " << nav_.utc_sbs[1] << " " << nav_.utc_sbs[2] << " "
+            << nav_.utc_sbs[3] << "\n";
+  std::cout << "ion_gps: " << nav_.ion_gps[0] << " " << nav_.ion_gps[1] << " " << nav_.ion_gps[2] << " "
+            << nav_.ion_gps[3] << "\n";
+  std::cout << "ion_gal: " << nav_.ion_gal[0] << " " << nav_.ion_gal[1] << " " << nav_.ion_gal[2] << " "
+            << nav_.ion_gal[3] << "\n";
+  std::cout << "ion_qzs: " << nav_.ion_qzs[0] << " " << nav_.ion_qzs[1] << " " << nav_.ion_qzs[2] << " "
+            << nav_.ion_qzs[3] << "\n";
+  std::cout << "ion_cmp: " << nav_.ion_cmp[0] << " " << nav_.ion_cmp[1] << " " << nav_.ion_cmp[2] << " "
+            << nav_.ion_cmp[3] << "\n";
 }
diff --git a/src/observations.cpp b/src/observations.cpp
index 4ef5448237b6e98ba5fc17f92e9f3da2cc3a6a6a..ea2dc15d658ce4644e8446ae71182ce21c428d4a 100644
--- a/src/observations.cpp
+++ b/src/observations.cpp
@@ -1,45 +1,167 @@
 #include "gnss_utils/gnss_utils.h"
 #include "gnss_utils/observations.h"
 
-
 using namespace GNSSUtils;
 
-
 Observations::Observations()
 {
+}
 
+Observations::Observations(const Observations& obs)
+  : sat_2_idx_(obs.sat_2_idx_), idx_2_sat_(obs.idx_2_sat_), obs_(obs.obs_.size())
+{
+  // copy all elements
+  for (auto i = 0; i < obs.obs_.size(); i++)
+  {
+    obs_[i].time      = obs.obs_[i].time;
+    obs_[i].eventime  = obs.obs_[i].eventime;
+    obs_[i].timevalid = obs.obs_[i].timevalid;
+    obs_[i].sat       = obs.obs_[i].sat;
+    obs_[i].rcv       = obs.obs_[i].rcv;
+    std::copy(obs.obs_[i].SNR, obs.obs_[i].SNR + NFREQ + NEXOBS, obs_[i].SNR);
+    std::copy(obs.obs_[i].LLI, obs.obs_[i].LLI + NFREQ + NEXOBS, obs_[i].LLI);
+    std::copy(obs.obs_[i].code, obs.obs_[i].code + NFREQ + NEXOBS, obs_[i].code);
+    std::copy(obs.obs_[i].qualL, obs.obs_[i].qualL + NFREQ + NEXOBS, obs_[i].qualL);
+    std::copy(obs.obs_[i].qualP, obs.obs_[i].qualP + NFREQ + NEXOBS, obs_[i].qualP);
+    obs_[i].freq = obs.obs_[i].freq;
+    std::copy(obs.obs_[i].L, obs.obs_[i].L + NFREQ + NEXOBS, obs_[i].L);
+    std::copy(obs.obs_[i].P, obs.obs_[i].P + NFREQ + NEXOBS, obs_[i].P);
+    std::copy(obs.obs_[i].D, obs.obs_[i].D + NFREQ + NEXOBS, obs_[i].D);
+  }
 }
 
 Observations::~Observations()
 {
-  this->obs_vector_.erase(obs_vector_.begin(), obs_vector_.end());
 }
 
 void Observations::clearObservations()
 {
-  this->obs_vector_.clear();
+  this->obs_.clear();
+  this->idx_2_sat_.clear();
+  this->sat_2_idx_.clear();
 }
 
-void Observations::pushObservation(obsd_t & obs)
+void Observations::addObservation(const obsd_t& obs)
 {
-  this->obs_vector_.push_back(obs);
+  assert(sat_2_idx_.size() == idx_2_sat_.size());
+  assert(sat_2_idx_.size() == obs_.size());
+
+  // copy obsd_t object
+  obsd_t copy_obs;
+  copy_obs.time      = obs.time;
+  copy_obs.eventime  = obs.eventime;
+  copy_obs.timevalid = obs.timevalid;
+  copy_obs.sat       = obs.sat;
+  copy_obs.rcv       = obs.rcv;
+  std::copy(obs.SNR, obs.SNR + NFREQ + NEXOBS, copy_obs.SNR);
+  std::copy(obs.LLI, obs.LLI + NFREQ + NEXOBS, copy_obs.LLI);
+  std::copy(obs.code, obs.code + NFREQ + NEXOBS, copy_obs.code);
+  std::copy(obs.qualL, obs.qualL + NFREQ + NEXOBS, copy_obs.qualL);
+  std::copy(obs.qualP, obs.qualP + NFREQ + NEXOBS, copy_obs.qualP);
+  copy_obs.freq = obs.freq;
+  std::copy(obs.L, obs.L + NFREQ + NEXOBS, copy_obs.L);
+  std::copy(obs.P, obs.P + NFREQ + NEXOBS, copy_obs.P);
+  std::copy(obs.D, obs.D + NFREQ + NEXOBS, copy_obs.D);
+
+  assert(!hasSatellite(copy_obs.sat) && "adding an observation of a satellite already stored!");
+  this->obs_.push_back(copy_obs);
+  idx_2_sat_.push_back(copy_obs.sat);
+  sat_2_idx_[copy_obs.sat] = obs_.size() - 1;
+
+  // std::cout << "added observation of sat " << (int)obs.sat << " stored in idx " << sat_2_idx_[obs.sat] << std::endl;
+
+  assert(sat_2_idx_.size() == idx_2_sat_.size());
+  assert(sat_2_idx_.size() == obs_.size());
+}
+
+void Observations::loadFromRinex(const std::string& rnx_file,
+                                 gtime_t            t_start,
+                                 gtime_t            t_end,
+                                 double             dt,
+                                 const char*        opt)
+{
+  obs_t obs;
+  obs.data = (obsd_t*)malloc(sizeof(obsd_t) * MAXSAT);
+  obs.n    = 0;
+  obs.nmax = MAXSAT;
+
+  // const char* opt = "";
+  auto stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, &obs, NULL, NULL);
+  if (stat == 1)
+    sortobs(&obs);
+  else
+  {
+    std::cout << "Observation: couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error") << stat << std::endl;
+    return;
+  }
+
+  for (int i = 0; i < obs.n; i++)
+  {
+    // std::cout << "time: " << time_str(obs.data[i].time, 3) <<  " | sat: " << int(obs.data[i].sat) << " | rcv: " <<
+    // int(obs.data[i].rcv) <<
+    //           " | SNR: " << int(obs.data[i].SNR[0]) << " | LLI: " << int(obs.data[i].LLI[0]) << " | code: " <<
+    //           int(obs.data[i].code[0]) <<
+    //             " | L: " << obs.data[i].L[0] <<  " | P: " << obs.data[i].P[0] << " | D: " << obs.data[i].D[0] <<
+    //             std::endl;
+    addObservation(obs.data[i]);
+  }
+
+  free(obs.data);
 }
 
-void Observations::reserveObservations(unsigned int n)
+void Observations::removeObservationByIdx(const int& _idx)
 {
-  this->obs_vector_.reserve(n);
+  // std::cout << "removing observation of idx " << _idx << std::endl;
+  assert(sat_2_idx_.size() == idx_2_sat_.size());
+  assert(sat_2_idx_.size() == obs_.size());
+  assert(obs_.size() > _idx);
+  assert(idx_2_sat_.size() > _idx);
+  assert(sat_2_idx_.count(idx_2_sat_.at(_idx)) != 0);
+
+  // remove obs from sat_2_idx map
+  sat_2_idx_.erase(idx_2_sat_.at(_idx));
+  // decrease the idx of the observations after the removed one
+  for (auto&& sat_idx : sat_2_idx_)
+    if (sat_idx.second > _idx)
+      sat_idx.second--;
+  // remove obs from obs and idx_2_sat vectors
+  obs_.erase(obs_.begin() + _idx);
+  idx_2_sat_.erase(idx_2_sat_.begin() + _idx);
+
+  assert(sat_2_idx_.size() == idx_2_sat_.size());
+  assert(sat_2_idx_.size() == obs_.size());
 }
 
-const std::vector<obsd_t> & Observations::getObservations() const
+void Observations::removeObservationBySat(const int& _sat)
 {
-  return this->obs_vector_;
+  // std::cout << "removing observation of sat " << _sat << std::endl;
+  assert(sat_2_idx_.size() == idx_2_sat_.size());
+  assert(sat_2_idx_.size() == obs_.size());
+  assert(sat_2_idx_.count(_sat) != 0);
+  assert(hasSatellite(_sat));
+  assert(obs_.size() > sat_2_idx_.at(_sat));
+  assert(idx_2_sat_.size() > sat_2_idx_.at(_sat));
+
+  int idx = sat_2_idx_.at(_sat);
+  // remove obs from sat_2_idx map
+  sat_2_idx_.erase(_sat);
+  // decrease the idx of the observations after the removed one
+  for (auto&& sat_idx : sat_2_idx_)
+    if (sat_idx.second > idx)
+      sat_idx.second--;
+  // remove obs from obs and idx_2_sat vectors
+  obs_.erase(obs_.begin() + idx);
+  idx_2_sat_.erase(idx_2_sat_.begin() + idx);
+
+  assert(sat_2_idx_.size() == idx_2_sat_.size());
+  assert(sat_2_idx_.size() == obs_.size());
 }
 
-void Observations::print(obsd_t & _obs)
+void Observations::print(obsd_t& _obs)
 {
   std::string msg = "Observation of satellite #" + std::to_string(_obs.sat);
   GNSSUtils::print(msg);
-  std::cout << "Time [s]: " << _obs.time.time << "\n";
+  std::cout << "Time [s]: " << _obs.time.time << " + " << _obs.time.sec << "\n";
   printArray("SNR: ", _obs.SNR, ARRAY_SIZE(_obs.SNR));
   printArray("LLI: ", _obs.LLI, ARRAY_SIZE(_obs.LLI));
   printArray("code: ", _obs.code, ARRAY_SIZE(_obs.code));
@@ -48,15 +170,52 @@ void Observations::print(obsd_t & _obs)
   printArray("D: ", _obs.D, ARRAY_SIZE(_obs.D));
 }
 
-void Observations::print(int _obs_idx)
+void Observations::printBySat(const int& _sat_number)
 {
-  print(obs_vector_[_obs_idx]);
+  print(getObservationBySat(_sat_number));
+}
+
+void Observations::printByIdx(const int& _idx)
+{
+  print(getObservationByIdx(_idx));
 }
 
 void Observations::print()
 {
-  for (auto obs: obs_vector_)
+  for (auto obs : obs_)
   {
     print(obs);
   }
-}
\ No newline at end of file
+}
+
+void Observations::findCommonObservations(const Observations& obs_1,
+                                          const Observations& obs_2,
+                                          Observations&       common_obs_1,
+                                          Observations&       common_obs_2)
+{
+  // clear and reserve
+  common_obs_1.clearObservations();
+  common_obs_2.clearObservations();
+
+  // std::cout << "obs 1 sats: ";
+  // for (auto&& obs_1_ref : obs_1.getObservations())
+  //    std::cout << (int)obs_1_ref.sat << " ";
+  // std::cout << std::endl;
+  // std::cout << "obs 2 sats: ";
+  // for (auto&& obs_2_ref : obs_2.getObservations())
+  //    std::cout << (int)obs_2_ref.sat << " ";
+  // std::cout << std::endl;
+
+  // iterate 1
+  for (auto&& obs_1_ref : obs_1.getObservations())
+    if (obs_2.hasSatellite(obs_1_ref.sat))
+    {
+      common_obs_1.addObservation(obs_1_ref);
+      common_obs_2.addObservation(obs_2.getObservationBySat(obs_1_ref.sat));
+    }
+
+  // std::cout << "common sats: ";
+  // for (auto&& obs_2_ref : common_obs_1.getObservations())
+  //    std::cout << (int)obs_2_ref.sat << " ";
+  // std::cout << std::endl;
+}
diff --git a/src/ublox_raw.cpp b/src/ublox_raw.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9cc2442d19ddaefe97e7741878a21a565fffbb50
--- /dev/null
+++ b/src/ublox_raw.cpp
@@ -0,0 +1,98 @@
+#include "gnss_utils/ublox_raw.h"
+
+using namespace GNSSUtils;
+
+UBloxRaw::UBloxRaw() : raw_data_type_(NO)
+{
+  if (init_raw(&raw_data_, STRFMT_UBX) == 0)
+  {
+    assert("Failed when allocating memory for raw_t");
+    return;
+  }
+};
+
+UBloxRaw::~UBloxRaw()
+{
+  free_raw(&raw_data_);
+};
+
+RawDataType UBloxRaw::addDataStream(const std::vector<u_int8_t>& data_stream)
+{
+  // Update type based on RTKLIB
+  for (auto data_byte = data_stream.begin(); data_byte != data_stream.end(); ++data_byte)
+  {
+    raw_data_type_ = input_ubx(&raw_data_, (unsigned char)*data_byte);
+
+    switch (raw_data_type_)
+    {
+      case NO:  //
+        // std::cout << "0 received!\n";
+        break;
+
+      case OBS:  // Observations
+        // std::cout << "Observations received!\n";
+        updateObservations();
+        // std::cout << "Observations updated!\n";
+        break;
+
+      case NAV_EPH:  // Ephemeris
+        std::cout << "Ephemeris received!\n";
+        nav_.copyEphemeris(raw_data_.nav);
+        // std::cout << "Ephemeris copied!\n";
+        break;
+
+      case NAV_SBAS:  // SBAS
+        std::cout << "SBAS received!\n";
+        nav_.addSbasMessage(raw_data_.sbsmsg);
+        // std::cout << "SBAS added!\n";
+        break;
+
+      case NAV_ALM:  // Almanac and ion/utc parameters
+        std::cout << "Almanac and ion/utc parameters received!\n";
+        nav_.freeAlmanac();
+        // std::cout << "Almanac freed!\n";
+        nav_.copyAlmanac(raw_data_.nav);
+        // std::cout << "Almanac copied!\n";
+        nav_.copyIonUtc(raw_data_.nav);
+        // std::cout << "ION UTC copied!\n";
+        break;
+
+      // Not handled messages
+      case NAV_ANT:
+        std::cout << "UBloxRaw: Received antenna postion parameters. Not handled.\n";
+        break;
+      case NAV_DGPS:
+        std::cout << "UBloxRaw: Received dgps correction. Not handled.\n";
+        break;
+      case NAV_SSR:
+        std::cout << "UBloxRaw: Received ssr message. Not handled.\n";
+        break;
+      case NAV_LEX:
+        std::cout << "UBloxRaw: Received lex message. Not handled.\n";
+        break;
+      case ERROR:
+        std::cout << "UBloxRaw: Received error message. Not handled.\n";
+        break;
+      default:
+        std::cout << "UBloxRaw: Received unknown message. Not handled.\n";
+        break;
+    }
+  }
+
+  return raw_data_type_;
+}
+
+void UBloxRaw::updateObservations()
+{
+  // sort and remove duplicated observations
+  sortobs(&raw_data_.obs);
+  // std::cout << "---------------------------JUST BEFORE!-------------------" << std::endl;
+  // obs_.print();
+
+  obs_.clearObservations();
+  for (int ii = 0; ii < raw_data_.obs.n; ++ii)
+    obs_.addObservation(raw_data_.obs.data[ii]);
+
+  // std::cout << "--------------------------JUST AFTER!---------------------" << std::endl;
+  // obs_.print();
+}
diff --git a/src/utils.cpp b/src/utils.cpp
index 3e41a0e2461e417c8e3ce267963f681d9ca6614e..cd697b39951aa113fb7a160f9b75722cbdf740f7 100644
--- a/src/utils.cpp
+++ b/src/utils.cpp
@@ -1,66 +1,64 @@
 #include "gnss_utils/utils.h"
 
-
 namespace GNSSUtils
 {
-void print(std::string & _msg) 
-  {
-    std::string msg = GNSSUTILS_MSG + _msg;
+void print(std::string& _msg)
+{
+  std::string msg = GNSSUTILS_MSG + _msg;
 
-    std::cout << msg << "\n";
-     
-  }
+  std::cout << msg << "\n";
+}
 
-  void printArray(std::string _name, int * _array, int size)
-  {  
-    std::cout << _name << ": [";
-    for (int ii=0; ii<size; ++ii)
-    {
-      std::cout << _array[ii];
-      if (ii==size-1) 
-        std::cout << "] \n";
-      else
-        std::cout << ",";
-    }
+void printArray(std::string _name, int* _array, int size)
+{
+  std::cout << _name << ": [";
+  for (int ii = 0; ii < size; ++ii)
+  {
+    std::cout << _array[ii];
+    if (ii == size - 1)
+      std::cout << "] \n";
+    else
+      std::cout << ",";
   }
+}
 
-  void printArray(std::string _name, unsigned char * _array, int size)
+void printArray(std::string _name, unsigned char* _array, int size)
+{
+  std::cout << _name << ": [";
+  for (int ii = 0; ii < size; ++ii)
   {
-    std::cout << _name << ": [";
-    for (int ii=0; ii<size; ++ii)
-    {
-      std::cout << (unsigned)_array[ii];
-      if (ii==size-1) 
-        std::cout << "] \n";
-      else
-        std::cout << ",";
-    }
+    std::cout << (int)(_array[ii]);
+    if (ii == size - 1)
+      std::cout << "] \n";
+    else
+      std::cout << ",";
   }
+}
 
-  void printArray(std::string _name, double * _array, int size)
-  {   
-    std::cout << _name << ": [";
-    for (int ii=0; ii<size; ++ii)
-    {
-      std::cout << _array[ii];
-      if (ii==size-1) 
-        std::cout << "] \n";
-      else
-        std::cout << ",";
-    }
+void printArray(std::string _name, double* _array, int size)
+{
+  std::cout << _name << ": [";
+  for (int ii = 0; ii < size; ++ii)
+  {
+    std::cout << _array[ii];
+    if (ii == size - 1)
+      std::cout << "] \n";
+    else
+      std::cout << ",";
   }
+}
 
-  void printArray(std::string _name, float * _array, int size)
-  {   
-    std::cout << _name << ": [";
-    for (int ii=0; ii<size; ++ii)
-    {
-      std::cout << _array[ii];
-      if (ii==size-1) 
-        std::cout << "] \n";
-      else
-        std::cout << ",";
-    }
+void printArray(std::string _name, float* _array, int size)
+{
+  std::cout << _name << ": [";
+  for (int ii = 0; ii < size; ++ii)
+  {
+    std::cout << _array[ii];
+    if (ii == size - 1)
+      std::cout << "] \n";
+    else
+      std::cout << ",";
   }
+}
 
-}
\ No newline at end of file
+}  // namespace GNSSUtils
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..93365929c3f20d1f751e1e5d6ff5476adf3a5249
--- /dev/null
+++ b/test/CMakeLists.txt
@@ -0,0 +1,23 @@
+# 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})
+
+# Observations test
+add_executable(gtest_observations gtest_observations.cpp)
+add_dependencies(gtest_observations libgtest)
+target_link_libraries(gtest_observations libgtest ${PROJECT_NAME})
+add_test(NAME gtest_observations COMMAND gtest_observations "${CMAKE_CURRENT_LIST_DIR}/../src/examples/sample_data.obs")
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3c3c48c74516488a8c4eb24c2492bd81a5edc7f9
--- /dev/null
+++ b/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/test/gtest/utils_gtest.h b/test/gtest/utils_gtest.h
new file mode 100644
index 0000000000000000000000000000000000000000..09c290758a8b953abc270a386ab4964075ebfdf8
--- /dev/null
+++ b/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/test/gtest_example.cpp b/test/gtest_example.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7dd3b9ceea4ff0b3c7ea1c6f8db3286b273f7b66
--- /dev/null
+++ b/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/test/gtest_navigation.cpp b/test/gtest_navigation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5170cf11310248899fd967b3b870f974a6368b2f
--- /dev/null
+++ b/test/gtest_navigation.cpp
@@ -0,0 +1,13 @@
+#include "gtest/utils_gtest.h"
+#include "gnss_utils/gnss_utils.h"
+
+TEST(NavigationTests, Whatever)
+{
+  
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
\ No newline at end of file
diff --git a/test/gtest_observations.cpp b/test/gtest_observations.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fc24115a0c7198f24b9b19718d219a9928ef78db
--- /dev/null
+++ b/test/gtest_observations.cpp
@@ -0,0 +1,75 @@
+#include "gtest/utils_gtest.h"
+#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/observations.h"
+
+using namespace GNSSUtils;
+
+std::string   rnx_file;
+obs_t         obs;
+const gtime_t t_start{ 0, 0 };  // no limit
+const gtime_t t_end{ 0, 0 };    // no limit
+const double  dt  = 0.0;        // no limit
+const char*   opt = "-SYS=G";   // only GPS | GPS+GAL: "-SYS=G,L" | ALL: ""
+
+void loadRinex()
+{
+  // RTKLIB utilities
+  obs.data = (obsd_t*)malloc(sizeof(obsd_t) * MAXSAT);
+  obs.n    = 0;
+  obs.nmax = MAXSAT;
+
+  int stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, &obs, NULL, NULL);
+
+  ASSERT_EQ(stat, 1);
+  sortobs(&obs);
+}
+
+TEST(ObservationsTest, AddClearObservation)
+{
+  loadRinex();
+
+  Observations observations;
+
+  // testing addition
+  for (int ii = 0; ii < obs.n; ++ii)
+  {
+    observations.addObservation(obs.data[ii]);
+    ASSERT_TRUE(equalObservations(obs.data[ii], observations.getObservations()[ii]));
+  }
+
+  ASSERT_TRUE(obs.n == observations.getObservations().size());
+
+  //Testing clear
+  observations.clearObservations();
+  ASSERT_TRUE(0 == observations.getObservations().size());
+
+}
+
+TEST(ObservationsTest, LoadFromRinex)
+{
+  // GNSSUtils utilities
+  Observations observations;
+  observations.loadFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+  observations.print();
+
+  // RTKLIB utilities
+  loadRinex();
+  ASSERT_EQ(obs.n, 6);
+
+  // Comparison
+  ASSERT_TRUE(obs.n == observations.getObservations().size());
+
+  for (int ii = 0; ii < obs.n; ++ii)
+  {
+    ASSERT_TRUE(equalObservations(obs.data[ii], observations.getObservations()[ii]));
+  }
+
+  free(obs.data);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  rnx_file = argv[1];
+  return RUN_ALL_TESTS();
+}
\ No newline at end of file
diff --git a/test/gtest_transformations.cpp b/test/gtest_transformations.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cae4e24bb4bd45e99fceaa07a201fe36926b5f78
--- /dev/null
+++ b/test/gtest_transformations.cpp
@@ -0,0 +1,200 @@
+#include "gtest/utils_gtest.h"
+#include "gnss_utils/gnss_utils.h"
+
+// Geodetic system parameters
+static double kSemimajorAxis = 6378137;
+static double kSemiminorAxis = 6356752.3142;
+static double kFirstEccentricitySquared = 6.69437999014 * 0.001;
+static double kSecondEccentricitySquared = 6.73949674228 * 0.001;
+
+using namespace GNSSUtils;
+
+TEST(TransformationsTest, ecefToLatLonAlt)
+{
+    Eigen::Vector3d ecef, latlonalt;
+
+    // aligned with ECEF X axis
+    ecef << 1e3, 0, 0;
+    latlonalt = ecefToLatLonAlt(ecef);
+
+    ASSERT_NEAR(latlonalt(0),0.0,1e-6);
+    ASSERT_NEAR(latlonalt(1),0.0,1e-6);
+
+    // aligned with ECEF Y axis
+    ecef << 0, 1e3, 0;
+    latlonalt = ecefToLatLonAlt(ecef);
+
+    ASSERT_NEAR(latlonalt(0),0.0,1e-6);
+    ASSERT_NEAR(latlonalt(1),M_PI / 2,1e-6);
+
+    // aligned with ECEF Z axis
+    ecef << 0, 0, 1e3;
+    latlonalt = ecefToLatLonAlt(ecef);
+
+    ASSERT_NEAR(latlonalt(0),M_PI / 2,1e-6);
+    ASSERT_NEAR(latlonalt(1),0.0,1e-6);
+}
+
+TEST(TransformationsTest, latLonAltToEcef)
+{
+    Eigen::Vector3d ecef, latlonalt;
+
+    // aligned with ECEF X axis
+    latlonalt << 0, 0, 0;
+    ecef = latLonAltToEcef(latlonalt);
+
+    ASSERT_NEAR(ecef(1),0.0,1e-6);
+    ASSERT_NEAR(ecef(2),0.0,1e-6);
+
+    // aligned with ECEF Y axis
+    latlonalt << 0, M_PI / 2, 0;
+    ecef = latLonAltToEcef(latlonalt);
+
+    ASSERT_NEAR(ecef(0),0.0,1e-6);
+    ASSERT_NEAR(ecef(2),0.0,1e-6);
+
+    // aligned with ECEF Z axis
+    latlonalt << M_PI / 2, 0, 0;
+    ecef = latLonAltToEcef(latlonalt);
+
+    ASSERT_NEAR(ecef(0),0.0,1e-6);
+    ASSERT_NEAR(ecef(1),0.0,1e-6);
+}
+
+TEST(TransformationsTest, latLonAltToEcef_ecefToLatLonAlt)
+{
+    Eigen::Vector3d ecef0, ecef1, latlonalt0, latlonalt1;
+
+    // ecef -> latlon -> ecef
+    for (auto i = 0; i<100; i++)
+    {
+        ecef0 = 1e3 * Eigen::Vector3d::Random();
+
+        latlonalt0 = ecefToLatLonAlt(ecef0);
+        ecef1 = latLonAltToEcef(latlonalt0);
+        ASSERT_MATRIX_APPROX(ecef0,ecef1,1e-6);
+    }
+    // latlon -> ecef -> latlon
+    for (auto i = 0; i<100; i++)
+    {
+        latlonalt0 = Eigen::Vector3d::Random();
+        latlonalt0(0) *= M_PI / 2;
+        latlonalt0(1) *= M_PI;
+        latlonalt0(2) *= 1e3;
+
+        ecef0 = latLonAltToEcef(latlonalt0);
+        latlonalt1 = ecefToLatLonAlt(ecef0);
+        ASSERT_MATRIX_APPROX(latlonalt0,latlonalt1,1e-6);
+    }
+}
+
+TEST(TransformationsTest, computeEnuEcef)
+{
+    Eigen::Vector3d t_ECEF_ENU, t_ENU_latlonalt, latlonalt1;
+    Eigen::Vector3d t_ENU_ECEF1,t_ENU_ECEF2,t_ENU_ECEF3;
+    Eigen::Matrix3d R_ENU_ECEF1,R_ENU_ECEF2,R_ENU_ECEF3;
+
+    // random
+    for (auto i = 0; i<100; i++)
+    {
+        t_ENU_latlonalt = Eigen::Vector3d::Random();
+        t_ENU_latlonalt(0) *= M_PI / 2;
+        t_ENU_latlonalt(1) *= M_PI;
+        t_ENU_latlonalt(2) *= 1e3;
+
+        t_ECEF_ENU = latLonAltToEcef(t_ENU_latlonalt);
+
+        // 1 from ECEF
+        computeEnuEcefFromEcef(t_ECEF_ENU, R_ENU_ECEF1, t_ENU_ECEF1);
+        // 2 from latlonalt
+        computeEnuEcefFromLatLonAlt(t_ENU_latlonalt, R_ENU_ECEF2, t_ENU_ECEF2);
+        // 3 Hardcoded solution
+        /* 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_ECEF3(0,0) = -sLon;
+        R_ENU_ECEF3(0,1) =  cLon;
+        R_ENU_ECEF3(0,2) =  0.0;
+
+        R_ENU_ECEF3(1,0) = -sLat*cLon;
+        R_ENU_ECEF3(1,1) = -sLat * sLon;
+        R_ENU_ECEF3(1,2) =  cLat;
+
+        R_ENU_ECEF3(2,0) =  cLat * cLon;
+        R_ENU_ECEF3(2,1) =  cLat * sLon;
+        R_ENU_ECEF3(2,2) =  sLat;
+
+        t_ENU_ECEF3 = -R_ENU_ECEF3*t_ECEF_ENU;
+
+        // CHECK
+        ASSERT_MATRIX_APPROX(t_ENU_ECEF1,t_ENU_ECEF2,1e-6);
+        ASSERT_MATRIX_APPROX(R_ENU_ECEF1,R_ENU_ECEF2,1e-6);
+        ASSERT_MATRIX_APPROX(t_ENU_ECEF1,t_ENU_ECEF3,1e-6);
+        ASSERT_MATRIX_APPROX(R_ENU_ECEF1,R_ENU_ECEF3,1e-6);
+    }
+}
+
+TEST(TransformationsTest, computeSatElevation)
+{
+    Eigen::Vector3d t_ENU_ECEF,t_ECEF_ENU,t_ENU_latlonalt,rec_sat_ENU, sat_ECEF;
+    Eigen::Matrix3d R_ENU_ECEF;
+
+    // random receiver position
+    for (auto i = 0; i<100; i++)
+    {
+        t_ENU_latlonalt = Eigen::Vector3d::Random();
+        t_ENU_latlonalt(0) *= M_PI / 2;
+        t_ENU_latlonalt(1) *= M_PI;
+        t_ENU_latlonalt(2) *= 1e3;
+
+        t_ECEF_ENU = latLonAltToEcef(t_ENU_latlonalt);
+
+        computeEnuEcefFromLatLonAlt(t_ENU_latlonalt, R_ENU_ECEF, t_ENU_ECEF);
+
+        // random elevation and heading
+        for (auto j = 0; j<100; j++)
+        {
+            Eigen::Vector2d rand2 = Eigen::Vector2d::Random();
+            double heading = rand2(0) * M_PI;
+            double elevation = rand2(0) * M_PI / 2;
+
+            rec_sat_ENU << cos(heading) * 1000, sin(heading) * 1000, tan(elevation) * 1000;
+
+            sat_ECEF = t_ECEF_ENU + R_ENU_ECEF.transpose() * rec_sat_ENU;
+
+            double elevation2 = computeSatElevation(t_ECEF_ENU, sat_ECEF);
+
+            ASSERT_NEAR(elevation, elevation2,1e-6);
+        }
+    }
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}