diff --git a/deps/RTKLIB b/deps/RTKLIB
index 4b8f505328faeb827fe7e0eb5adac1dddaf39cf4..eac44a45c19ac56c0c031135e9818788b5993005 160000
--- a/deps/RTKLIB
+++ b/deps/RTKLIB
@@ -1 +1 @@
-Subproject commit 4b8f505328faeb827fe7e0eb5adac1dddaf39cf4
+Subproject commit eac44a45c19ac56c0c031135e9818788b5993005
diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index ec75472312f3add20a8730ce79abace251d5e674..5ba6069e9ace935c1f2b347e93b82af29f741862 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/gnss_utils.h
@@ -40,7 +40,7 @@ namespace GNSSUtils
   };
 
   ComputePosOutput computePos(const Observations & _observations,
-                              Navigation & _navigation,
+                              const Navigation & _navigation,
                               const prcopt_t & _prcopt);
   
   // ComputePosOutput computePosOwn(const Observations & _observations,
diff --git a/include/gnss_utils/navigation.h b/include/gnss_utils/navigation.h
index 54a3073702553733e054bcd7259a199c352a45d3..846e7c6a2ce20801940586675ef0c09b1392063d 100644
--- a/include/gnss_utils/navigation.h
+++ b/include/gnss_utils/navigation.h
@@ -22,6 +22,7 @@ class Navigation
     public:
         // Constructor & Destructor
         Navigation();
+        Navigation(const Navigation& nav);
         ~Navigation();
 
         // Public objects
@@ -48,17 +49,9 @@ class Navigation
         void clearSBASEphemeris();
         void clearAlmanac();
 
-        //void allocateEphemeris(int n_sat = MAXSAT);
-        //void deleteEphemeris();
+        void copyEphemeris(const nav_t& nav);        
+        void copyAlmanac(const nav_t& nav);        
 
-        //void allocateGLONASSEphemeris(int n_sat = NSATGLO);
-        //void deleteGLONASSEphemeris();
-
-        //void allocateSBASEphemeris(int n_sat = NSATSBS*2); //SBAS
-        //void deleteSBASEphemeris();
-
-        //void allocateAlmanac(int n_sat = MAXSAT);
-        //void deleteAlmanac();
         void print();
 
     private:
diff --git a/include/gnss_utils/ublox_raw.h b/include/gnss_utils/ublox_raw.h
index 46e21c321efd8ea7983d0b248532b036eeb15ec6..2075544df2fa3360ea04214fea5e13d6577d4703 100644
--- a/include/gnss_utils/ublox_raw.h
+++ b/include/gnss_utils/ublox_raw.h
@@ -6,6 +6,8 @@
 namespace GNSSUtils
 {
 
+enum RawDataType {OBS = 1, NAV_EPH = 2, NAV_SBAS = 3, NAV_ALM = 9};
+
 class UBloxRaw
 {
   public:
@@ -17,8 +19,7 @@ class UBloxRaw
   Observations getObservations();
   Navigation getNavigation();
 
-  inline bool newObsData(){return new_obs_;};
-  inline bool newNavData(){return new_nav_;};
+  const int& getRawDataType() const;
 
   private:
   raw_t raw_data_;
@@ -26,8 +27,7 @@ class UBloxRaw
   Observations obs_;
   Navigation nav_;
 
-  bool new_obs_;
-  bool new_nav_;
+  int raw_data_type_;
   
   void updateObservations();  
 
diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp
index 0e6409c15f96d6d6cfd5e8e372887e68c634fabe..91099bd5899a1cd1221cc8b727e59bb037021055 100644
--- a/src/gnss_utils.cpp
+++ b/src/gnss_utils.cpp
@@ -4,7 +4,7 @@ namespace GNSSUtils
 {
   
   ComputePosOutput computePos(const GNSSUtils::Observations & _observations,
-                                    GNSSUtils::Navigation & _navigation,
+                              const GNSSUtils::Navigation & _navigation,
                                     const prcopt_t & _prcopt)
   {
 
@@ -30,7 +30,7 @@ namespace GNSSUtils
     output.time = sol.time.time;
     output.sec = sol.time.sec;
     output.pos  = Eigen::Vector3d(sol.rr);
-    // std::cout << "Compute pos:  " << output.pos.transpose() << "\n";
+    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],
@@ -41,7 +41,6 @@ namespace GNSSUtils
     // {
     //   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;
diff --git a/src/navigation.cpp b/src/navigation.cpp
index 8376460d00b5af67dd02f0a6b11475413f2d6a09..6784c86b3ee9bdbd6eb85ddfc1aff3859b80c410 100644
--- a/src/navigation.cpp
+++ b/src/navigation.cpp
@@ -3,31 +3,48 @@
 
 using namespace GNSSUtils;
 
-
 Navigation::Navigation()
 {
+  nav_.eph =NULL;
+  nav_.geph=NULL;
+  nav_.seph=NULL;
+  nav_.peph=NULL;
+  nav_.pclk=NULL;
+  nav_.alm =NULL;
+  nav_.tec =NULL;
+  nav_.fcb =NULL; 
+  nav_.n =nav_.nmax =0;
+  nav_.ng=nav_.ngmax=0;
+  nav_.ns=nav_.nsmax=0;
+  nav_.ne=nav_.nemax=0;
+  nav_.nc=nav_.ncmax=0;
+  nav_.na=nav_.namax=0;
+  nav_.nt=nav_.ntmax=0;
+  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; 
-    nav_.n =nav_.nmax =0;
-    nav_.ng=nav_.ngmax=0;
-    nav_.ns=nav_.nsmax=0;
-    nav_.ne=nav_.nemax=0;
-    nav_.nc=nav_.ncmax=0;
-    nav_.na=nav_.namax=0;
-    nav_.nt=nav_.ntmax=0;
-    nav_.nf=nav_.nfmax=0;
-
-//    allocateEphemeris(0);
-//    allocateGLONASSEphemeris(0);
-//    allocateSBASEphemeris(0);
-//    allocateAlmanac(0);
+Navigation::Navigation(const Navigation& nav)
+{
+  nav_.eph =NULL;
+  nav_.geph=NULL;
+  nav_.seph=NULL;
+  nav_.peph=NULL;
+  nav_.pclk=NULL;
+  nav_.alm =NULL;
+  nav_.tec =NULL;
+  nav_.fcb =NULL; 
+  nav_.n =nav_.nmax =0;
+  nav_.ng=nav_.ngmax=0;
+  nav_.ns=nav_.nsmax=0;
+  nav_.ne=nav_.nemax=0;
+  nav_.nc=nav_.ncmax=0;
+  nav_.na=nav_.namax=0;
+  nav_.nt=nav_.ntmax=0;
+  nav_.nf=nav_.nfmax=0;
+
+  nav_t nav_cpy = nav.getNavigation();
+  copyEphemeris(nav_cpy);
+  copyAlmanac(nav_cpy);
 }
 
 Navigation::~Navigation()
@@ -38,6 +55,27 @@ Navigation::~Navigation()
 void Navigation::clearNavigation()
 {  
   freenav(&nav_,255);
+  // int opt = 255;
+  // if (opt&0x01) {free(nav_.eph ); nav_.eph =NULL; nav_.n =nav_.nmax =0;}
+  // if (opt&0x02) {free(nav_.geph); nav_.geph=NULL; nav_.ng=nav_.ngmax=0;}
+  // if (opt&0x04) {free(nav_.seph); nav_.seph=NULL; nav_.ns=nav_.nsmax=0;}
+  // if (opt&0x08) {free(nav_.peph); nav_.peph=NULL; nav_.ne=nav_.nemax=0;}
+  // if (opt&0x10) {free(nav_.pclk); nav_.pclk=NULL; nav_.nc=nav_.ncmax=0;}
+  // if (opt&0x20)
+  // {
+  //   std::cout << "Here" << std::endl;
+  //   if (nav_.alm == NULL)
+  //     printf("--------------THIS POINTER IS NULL!!--------");
+
+  //   printf("Pointer address almanac: %p \n", nav_.alm);
+  //   free(nav_.alm);
+  //   std::cout << "Here 1" << std::endl;
+  //   nav_.alm = NULL; 
+  //   std::cout << "Here 2" << std::endl;
+  //   nav_.na=nav_.namax=0;
+  // }
+  // if (opt&0x40) {free(nav_.tec ); nav_.tec =NULL; nav_.nt=nav_.ntmax=0;}
+  // if (opt&0x80) {free(nav_.fcb ); nav_.fcb =NULL; nav_.nf=nav_.nfmax=0;}
 }
 
 void Navigation::setNavigation(nav_t _nav)
@@ -164,74 +202,43 @@ void Navigation::clearAlmanac()
   if (nav_.alm != NULL) {free(nav_.alm ); nav_.alm =NULL; nav_.na=nav_.namax=0;}
 }
 
-/****************** 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;
-//}
-//void Navigation::deleteEphemeris()
-//{
-//    std::cout << "Navigation::deleteEphemeris...\n";
-//    free(nav_.eph);
-//    nav_.eph =NULL;
-//    nav_.n = 0;
-//    nav_.nmax =0;
-//    std::cout << "deleted!\n";
-//}
-
-//void Navigation::allocateGLONASSEphemeris(int n_sat)
-//{
-//  geph_t geph0={0,-1};
-//  int i;
-//  nav_.geph = (geph_t *)malloc(sizeof(geph_t)*n_sat);
-//}
-//void Navigation::deleteGLONASSEphemeris()
-//{
-//    std::cout << "Navigation::deleteGLONASSEphemeris...\n";
-//  free(nav_.geph);
-//  nav_.geph =NULL;
-//  nav_.ng = 0;
-//  nav_.ngmax =0;
-//  std::cout << "deleted!\n";
-//}
-
-//void Navigation::allocateSBASEphemeris(int n_sat)
-//{
-//  seph_t seph0={0};
-//  int i;
-//
-//  nav_.seph = (seph_t *)malloc(sizeof(seph_t)*n_sat);
-//  for (i=0; i<n_sat; i++) nav_.seph[i] = seph0;
-//}
-//void Navigation::deleteSBASEphemeris()
-//{
-//    std::cout << "Navigation::deleteSBASEphemeris...\n";
-//  free(nav_.seph);
-//  nav_.seph =NULL;
-//  nav_.ns = 0;
-//  nav_.nsmax =0;
-//  std::cout << "deleted!\n";
-//}
-
-//void Navigation::allocateAlmanac(int n_sat)
-//{
-//  alm_t  alm0 ={0,-1};
-//  int i;
-//
-//  nav_.alm = (alm_t *)malloc(sizeof(alm_t)*n_sat);
-//  for (i=0; i<n_sat; i++) nav_.alm[i] = alm0;
-//}
-//void Navigation::deleteAlmanac()
-//{
-//    std::cout << "Navigation::deleteAlmanac...\n";
-//  free(nav_.alm);
-//  nav_.na = 0;
-//  nav_.namax = 0;
-//  std::cout << "deleted!\n";
-//}
+void Navigation::copyEphemeris(const nav_t& nav)
+{
+  if (nav.eph != NULL)
+  {
+    for (int ii = 0; ii < nav.n; ++ii)
+    {
+      addEphemeris(nav.eph[ii]);
+    }
+  }
+
+  if (nav.geph != NULL)
+  {
+    for (int ii = 0; ii < nav.ng; ++ii)
+    {
+      addGLONASSEphemeris(nav.geph[ii]);
+    }
+  }
+
+  if (nav.seph != NULL)
+  {
+    for (int ii = 0; ii < nav.ns; ++ii)
+    {
+      addSBASEphemeris(nav.seph[ii]);
+    }
+  }
+}
+
+void Navigation::copyAlmanac(const nav_t& nav)
+{
+  if (nav.alm != NULL)
+  {
+    for (int ii = 0; ii < nav.na; ++ii)
+    {
+      addAlmanac(nav.alm[ii]);
+    }
+  }
+}
 
 void Navigation::print()
 {
diff --git a/src/ublox_raw.cpp b/src/ublox_raw.cpp
index ff396324c4c8bb3df491c6e683ce144615081661..a6613cae851c9ea86b83aec3eb7cbc25a1690746 100644
--- a/src/ublox_raw.cpp
+++ b/src/ublox_raw.cpp
@@ -10,8 +10,7 @@ UBloxRaw::UBloxRaw()
     return;
   }
 
-  new_obs_ = false;
-  new_nav_ = false;
+  raw_data_type_ = 0;
 };
 
 UBloxRaw::~UBloxRaw(){};
@@ -19,62 +18,56 @@ UBloxRaw::~UBloxRaw(){};
 int UBloxRaw::addDataStream(const std::vector<u_int8_t>& data_stream)
 {
   // Update type based on RTKLIB
-  int update_type = 0;
   for (auto data_byte = data_stream.begin(); data_byte != data_stream.end(); ++data_byte)
   {
-    update_type = input_ubx(&raw_data_, (unsigned char)*data_byte);
+    raw_data_type_ = input_ubx(&raw_data_, (unsigned char)*data_byte);
   }
 
-  switch (update_type)
+  switch (raw_data_type_)
   {
   case 1:
     updateObservations();
-    new_obs_ = true;
     break;
   case 2:
     // Ephemeris
     nav_.clearEphemeris();
     nav_.clearGLONASSEphemeris();
     nav_.clearSBASEphemeris();
-    nav_.addEphemeris(*(raw_data_.nav.eph));
-    nav_.addGLONASSEphemeris(*(raw_data_.nav.geph));
-    nav_.addSBASEphemeris(*(raw_data_.nav.seph));
-    new_nav_ = true;
+    nav_.copyEphemeris(raw_data_.nav);
     break;
   case 3:
     // SBAS
     nav_.clearEphemeris();
     nav_.clearGLONASSEphemeris();
     nav_.clearSBASEphemeris();
-    nav_.addEphemeris(*(raw_data_.nav.eph));
-    nav_.addGLONASSEphemeris(*(raw_data_.nav.geph));
-    nav_.addSBASEphemeris(*(raw_data_.nav.seph));
+    nav_.copyEphemeris(raw_data_.nav);
     updateObservations();
-    new_obs_ = true;
-    new_nav_ = true;
     break;
   case 9:
     // Almanac
     nav_.clearAlmanac();
-    nav_.addAlmanac(*(raw_data_.nav.alm));
-    new_nav_ = true;
+    nav_.copyAlmanac(raw_data_.nav);
     break;
   }
 
-  return update_type;
+  return raw_data_type_;
 }
 
 Observations UBloxRaw::getObservations() 
 {
-  new_obs_ = false;
   return obs_;
 }
+
 Navigation UBloxRaw::getNavigation() 
 {
-  new_nav_ = false;
   return nav_;
 }
 
+const int& UBloxRaw::getRawDataType() const
+{
+  return raw_data_type_;
+}
+
 void UBloxRaw::updateObservations()
 {
   // std::cout << "---------------------------JUST BEFORE!-------------------" << std::endl;