diff --git a/include/gnss_utils/TDCP.h b/include/gnss_utils/TDCP.h
index 3580569f867b31bee90ae83d67d012a16f0b1233..439a981ad097ea568d1cda9321b31c9e1b86fa55 100644
--- a/include/gnss_utils/TDCP.h
+++ b/include/gnss_utils/TDCP.h
@@ -17,47 +17,69 @@
 
 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);
-
-}
+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/navigation.h b/include/gnss_utils/navigation.h
index ba4c3f594f6fa59e852575fb87d9d6c49b4cf4da..20f65a0ffb1e111a5ba68f5a1a4f22693e73b426 100644
--- a/include/gnss_utils/navigation.h
+++ b/include/gnss_utils/navigation.h
@@ -5,364 +5,361 @@
 #include <iostream>
 #include <memory>
 
-extern "C"
-{
-    #include "rtklib.h"
+extern "C" {
+#include "rtklib.h"
 }
 
 namespace GNSSUtils
 {
-
 class Navigation;
-typedef std::shared_ptr<Navigation> NavigationPtr;
+typedef std::shared_ptr<Navigation>       NavigationPtr;
 typedef std::shared_ptr<const Navigation> NavigationConstPtr;
 
 class Navigation
 {
-    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
+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
+inline void Navigation::uniqueNavigation()  // remove duplicated ephemerides and update wave lengths
 {
-    uniqnav(&nav_);
+  uniqnav(&nav_);
 }
 
-inline const nav_t & Navigation::getNavigation() const
+inline const nav_t& Navigation::getNavigation() const
 {
   return nav_;
 }
 
-inline nav_t & Navigation::getNavigation()
+inline nav_t& Navigation::getNavigation()
 {
   return nav_;
 }
 
-inline bool Navigation::addEphemeris(const eph_t &eph)
+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;
+  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;
+  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;
+  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);
+  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);
+  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;
+  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);
+  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);
+  return addToArray<fcbd_t>(fcb, nav_.fcb, nav_.nf, nav_.nfmax);
 }
 
-inline void Navigation::addSbasMessage(const sbsmsg_t &sbas_msg)
+inline void Navigation::addSbasMessage(const sbsmsg_t& sbas_msg)
 {
-    sbsupdatecorr(&sbas_msg, &nav_);
+  sbsupdatecorr(&sbas_msg, &nav_);
 }
 
-
 inline void Navigation::freeEphemeris()
 {
-    freeEph(nav_);
+  freeEph(nav_);
 }
 
 inline void Navigation::freeGlonassEphemeris()
 {
-    freeGeph(nav_);
+  freeGeph(nav_);
 }
 
 inline void Navigation::freePreciseEphemeris()
 {
-    freePeph(nav_);
+  freePeph(nav_);
 }
 
 inline void Navigation::freeSbasEphemeris()
 {
-    freeSeph(nav_);
+  freeSeph(nav_);
 }
 
 inline void Navigation::freePreciseClock()
 {
-    freePclk(nav_);
+  freePclk(nav_);
 }
 
 inline void Navigation::freeTecData()
 {
-    freeTec(nav_);
+  freeTec(nav_);
 }
 
 inline void Navigation::freeFcbData()
 {
-    freeFcb(nav_);
+  freeFcb(nav_);
 }
 
 inline void Navigation::freeAlmanac()
 {
-    freeAlm(nav_);
+  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)
+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)))
     {
-        //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;
+      printf("addToArray malloc error: n=%d\n", nmax);
+      free(array);
+      array = NULL;
+      n = nmax = 0;
+      return false;
     }
-    //std::cout << "addToArray: adding element " << n << "\n";
-    array[n++] = new_element;
-    //std::cout << "addToArray: added!\n";
-    return true;
+    // 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;
 }
 
-template<typename T>
-inline bool Navigation::copyArray(const T *array_in, const int &n_in, T *&array_out, int &n_out, int &nmax_out)
+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;
+  // std::cout << "copyArray: " << n_in << " elements\n";
+  if (array_in == NULL)
+    return false;
 
-    //std::cout << "copyArray: array in not null\n";
+  // std::cout << "copyArray: array in not null\n";
 
-    for (int i = 0; i<n_in; i++)
+  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: 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: failed to add..\n";
+      return false;
     }
+    // std::cout << "copyArray: n_out = " << n_out << " nmax_out = " << nmax_out << "\n";
+  }
 
-    //std::cout << "copyArray: all copied\n";
+  // std::cout << "copyArray: all copied\n";
 
-    return true;
+  return true;
 }
 
-template<typename T>
-void Navigation::freeArray(T *&array, int &n, int &nmax)
+template <typename T>
+void Navigation::freeArray(T*& array, int& n, int& nmax)
 {
-    if (array != NULL)
-        free(array);
-    array = NULL;
-    n = nmax = 0;
+  if (array != NULL)
+    free(array);
+  array = NULL;
+  n = nmax = 0;
 }
 
-inline void Navigation::freeEph(nav_t &nav)
+inline void Navigation::freeEph(nav_t& nav)
 {
-    freeArray<eph_t>(nav.eph, nav.n, nav.nmax);
+  freeArray<eph_t>(nav.eph, nav.n, nav.nmax);
 }
 
-inline void Navigation::freeGeph(nav_t &nav)
+inline void Navigation::freeGeph(nav_t& nav)
 {
-    freeArray<geph_t>(nav.geph, nav.ng, nav.ngmax);
+  freeArray<geph_t>(nav.geph, nav.ng, nav.ngmax);
 }
 
-inline void Navigation::freeSeph(nav_t &nav)
+inline void Navigation::freeSeph(nav_t& nav)
 {
-    freeArray<seph_t>(nav.seph, nav.ns, nav.nsmax);
+  freeArray<seph_t>(nav.seph, nav.ns, nav.nsmax);
 }
 
-inline void Navigation::freePeph(nav_t &nav)
+inline void Navigation::freePeph(nav_t& nav)
 {
-    freeArray<peph_t>(nav.peph, nav.ne, nav.nemax);
+  freeArray<peph_t>(nav.peph, nav.ne, nav.nemax);
 }
 
-inline void Navigation::freeAlm(nav_t &nav)
+inline void Navigation::freeAlm(nav_t& nav)
 {
-    freeArray<alm_t>(nav.alm, nav.na, nav.namax);
+  freeArray<alm_t>(nav.alm, nav.na, nav.namax);
 }
 
-inline void Navigation::freePclk(nav_t &nav)
+inline void Navigation::freePclk(nav_t& nav)
 {
-    freeArray<pclk_t>(nav.pclk, nav.nc, nav.ncmax);
+  freeArray<pclk_t>(nav.pclk, nav.nc, nav.ncmax);
 }
 
-inline void Navigation::freeTec(nav_t &nav)
+inline void Navigation::freeTec(nav_t& nav)
 {
-    freeArray<tec_t>(nav.tec, nav.nt, nav.ntmax);
+  freeArray<tec_t>(nav.tec, nav.nt, nav.ntmax);
 }
 
-inline void Navigation::freeFcb(nav_t &nav)
+inline void Navigation::freeFcb(nav_t& nav)
 {
-    freeArray<fcbd_t>(nav.fcb, nav.nf, nav.nfmax);
+  freeArray<fcbd_t>(nav.fcb, nav.nf, nav.nfmax);
 }
 
-inline void Navigation::freeNavArrays(nav_t &nav)
+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);
+  // 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 f0715dc58893ee4fe3e3b48868089cbc5c480923..5610a7ec08698c19a6cf84152da0773b83f55cb2 100644
--- a/include/gnss_utils/observations.h
+++ b/include/gnss_utils/observations.h
@@ -9,127 +9,127 @@
 
 #include "gnss_utils/utils.h"
 
-extern "C"
-{
-    #include "rtklib.h"
+extern "C" {
+#include "rtklib.h"
 }
 
 namespace GNSSUtils
 {
-
 class Observations;
-typedef std::shared_ptr<Observations> ObservationsPtr;
+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:
+  // 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 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 removeObservationByIdx(const int& _idx);
+  void removeObservationBySat(const int& _sat);
 
-        void removeObservationByIdx(const int& _idx);
-        void removeObservationBySat(const int& _sat);
+  std::vector<obsd_t>&       getObservations();
+  const std::vector<obsd_t>& getObservations() const;
 
-        std::vector<obsd_t>& getObservations();
-        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;
 
-        obsd_t& getObservationBySat(const unsigned char& sat_number);
-        const obsd_t& getObservationBySat(const unsigned char& sat_number) const;
+  obsd_t&       getObservationByIdx(const int& idx);
+  const obsd_t& getObservationByIdx(const int& idx) const;
 
-        obsd_t& getObservationByIdx(const int& idx);
-        const obsd_t& getObservationByIdx(const int& idx) const;
+  obsd_t*       data();
+  const obsd_t* data() const;
 
-        obsd_t* data();
-        const obsd_t* data() const;
+  size_t size() const;
 
-        size_t size() const;
+  bool hasSatellite(const unsigned char& i) const;
 
-        bool hasSatellite(const unsigned char& i) const;
+  void print(obsd_t& _obs);
+  void printBySat(const int& _sat);
+  void printByIdx(const int& _idx);
+  void print();
 
-        void print(obsd_t & _obs);
-        void printBySat(const int& _sat);
-        void printByIdx(const int& _idx);
-        void print();
+  static void findCommonObservations(const Observations& obs_1,
+                                     const Observations& obs_2,
+                                     Observations&       common_obs_1,
+                                     Observations&       common_obs_2);
 
-        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:
-        // 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
+  // Private methods
 };
 
 inline const std::vector<obsd_t>& Observations::getObservations() const
 {
-    return this->obs_;
+  return this->obs_;
 }
 
 inline std::vector<obsd_t>& Observations::getObservations()
 {
-    return this->obs_;
+  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));
+  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));
+  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);
+  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);
+  assert(obs_.size() > idx);
+  return obs_.at(idx);
 }
 
 inline obsd_t* Observations::data()
 {
-    return obs_.data();
+  return obs_.data();
 }
 
 inline const obsd_t* Observations::data() const
 {
-    return obs_.data();
+  return obs_.data();
 }
 
 inline size_t Observations::size() const
 {
-    return obs_.size();
+  return obs_.size();
 }
 
 inline bool Observations::hasSatellite(const unsigned char& i) const
 {
-    return sat_2_idx_.count(i) != 0;
+  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
index cc9f65be15cadcb21aebf8f9505dbf4931e54008..929ccd8767017382bd520780e62e4c32140be6fb 100644
--- a/include/gnss_utils/ublox_raw.h
+++ b/include/gnss_utils/ublox_raw.h
@@ -5,42 +5,52 @@
 
 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};
+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:
+public:
   UBloxRaw();
   ~UBloxRaw();
 
   RawDataType addDataStream(const std::vector<u_int8_t>& data_stream);
-  
+
   Observations& getObservations();
-  Navigation& getNavigation();
+  Navigation&   getNavigation();
 
   RawDataType getRawDataType() const;
 
-  private:
+private:
   raw_t raw_data_;
 
   Observations obs_;
-  Navigation nav_;
+  Navigation   nav_;
 
   RawDataType raw_data_type_;
-  
-  void updateObservations();  
 
+  void updateObservations();
 };
 
 inline GNSSUtils::Observations& UBloxRaw::getObservations()
 {
-    return obs_;
+  return obs_;
 }
 
 inline GNSSUtils::Navigation& UBloxRaw::getNavigation()
 {
-    return nav_;
+  return nav_;
 }
 
 inline RawDataType UBloxRaw::getRawDataType() const
@@ -48,7 +58,6 @@ 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/TDCP.cpp b/src/TDCP.cpp
index 033e4925f159d66f96edff9d197f1b946f4cfd2e..66e5a10997152c4544c7b6d1e5eaefa2cfa6028f 100644
--- a/src/TDCP.cpp
+++ b/src/TDCP.cpp
@@ -9,548 +9,571 @@
 
 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)
+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);
+  // 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)
+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);
+  // 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)
+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());
+  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);
 
-    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();
 
-    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;
+  }
 
-    if(n_common_sats < required_n_sats)
+  // 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)
     {
-        #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;
+      // 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++;
+      }
     }
-
-    // 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++)
+    else
+        // L1/E1
+        if (std::abs(obs_r.P[0]) > 1e-12 and std::abs(obs_k.P[0]) > 1e-12)
     {
-        auto&& obs_r = common_obs_r.getObservationByIdx(obs_idx);
-        auto&& obs_k = common_obs_k.getObservationByIdx(obs_idx);
+      row_2_sat_freq[row] = std::make_pair(sat_number, 0);
+      row++;
+    }
+  }
+  int n_differences = row_2_sat_freq.size();
 
-        const int& sat_number = obs_k.sat;
+  // Initial guess
+  Eigen::Vector4d d_0(d), delta_d(Eigen::Vector4d::Zero());
 
-        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++;
-            }
+#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;
     }
-    int n_differences = row_2_sat_freq.size();
 
-    // Initial guess
-    Eigen::Vector4d d_0(d), delta_d(Eigen::Vector4d::Zero());
+    // 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 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
+    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];
 
-    // 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;
+#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
 
-    for (auto row_sat_freq_it : row_2_sat_freq)
+    if (!sd_params.relinearize_jacobian)
     {
-        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;
+      // 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;
+    }
+  }
 
-        auto obs_r = common_obs_r.getObservationBySat(sat_number);
-        auto obs_k = common_obs_k.getObservationBySat(sat_number);
+  // 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;
+      }
+    }
 
-        // excluded satellite
-        if (discarded_sats.count(sat_number) != 0)
-        {
-            #if GNSS_UTILS_TDCP_DEBUG == 1
-            std::cout << "\tdiscarded sat" << std::endl;
-            #endif
-            continue;
-        }
+    // Solve
+    cov_d   = (A.transpose() * A).inverse();
+    delta_d = -cov_d * A.transpose() * r;
+    d += delta_d;
 
-        // 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;
-        }
+    // wrong solution
+    if (d.array().isNaN().any())
+    {
+      std::cout << "TDCP: LSM DID NOT WORK. NAN values appeared. ABORTING!!" << std::endl;
+      return false;
     }
 
-    // LEAST SQUARES SOLUTION =======================================================================
-    for (int j = 0; j < sd_params.max_iterations; j++)
+    // 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)
     {
-        // fill A and r
-        for (int row = 0; row < A.rows(); row++)
+      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++)
         {
-            // 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);
+          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());
 
-            // Evaluate A
-            if (sd_params.relinearize_jacobian)
+#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)
             {
-                // 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;
+              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);
         }
 
-        // Solve
-        cov_d = (A.transpose()*A).inverse();
-        delta_d = -cov_d*A.transpose()*r;
-        d += delta_d;
+        // No successful RAIM solution
+        if (worst_sat_row == -1)
+        {
+          printf("TDCP: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!");
+          return false;
+        }
 
-        // wrong solution
-        if(d.array().isNaN().any())
+        // 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
         {
-            std::cout << "TDCP: LSM DID NOT WORK. NAN values appeared. ABORTING!!" << std::endl;
-            return false;
+          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);
         }
 
-        // 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;
+        // apply the RAIM solution
+        d     = best_d;
+        cov_d = (A.transpose() * A).inverse();
 
 #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));
+        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
-
-
-        // 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));
+      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
-                        // 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;
+#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)
+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;
+  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::set<int> remove_sats;
+  // std::cout << "filterCommonSatellites: initial size: " << common_obs_k.size() << std::endl;
 
-    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;
+  std::set<int> remove_sats;
 
-        // already discarded sats
-        if (discarded_sats.count(sat_number) != 0)
-        {
-            remove_sats.insert(sat_number);
-            continue;
-        }
+  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;
 
-        // 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;
-        }
+    // already discarded sats
+    if (discarded_sats.count(sat_number) != 0)
+    {
+      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;
-        }
+    // 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;
     }
 
-    // remove sats
-    //std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
-    for (auto sat : remove_sats)
+    // 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))
     {
-        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);
+      // 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;
     }
 
-    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());
+    // 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;
+    }
 
-    //std::cout << "final size: " << common_obs_k.size() << std::endl;
-}
+    // 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 fb0792acec09adc79a13e7ea13c1914c96226018..6b66334719c894be87acb930224220337162f1c5 100644
--- a/src/examples/gnss_utils_test.cpp
+++ b/src/examples/gnss_utils_test.cpp
@@ -8,32 +8,29 @@
 
 #include <iostream>
 
-extern "C"
-{
-  #include "../deps/RTKLIB/src/rinex.c"
+extern "C" {
+#include "../deps/RTKLIB/src/rinex.c"
 }
 
 using namespace GNSSUtils;
 
-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;
 
-
-  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 = "-SYS=G"; // only GPS | GPS+GAL: "-SYS=G,L" | ALL: ""
+  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 = "-SYS=G";   // only GPS | GPS+GAL: "-SYS=G,L" | ALL: ""
 
   // load observations from RINEX file
   observations.loadFromRinex("../src/examples/raw_201805171357.obs", t_start, t_end, dt, opt);
@@ -51,26 +48,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 = 0.525;  // 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] = 0;//4789374.0336;
-  prcopt.ru[1] = 0;//177048.3292;
-  prcopt.ru[2] = 0;//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);
+  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;
@@ -79,9 +76,9 @@ int main(int argc, char *argv[])
 
   int stat;
 
-  sol_t solb={{0}};
+  sol_t solb = { { 0 } };
 
-  char msg[128]="";
+  char msg[128] = "";
 
   stat = pntpos(observations.data(), observations.size(), &navigation.getNavigation(), &prcopt, &solb, NULL, NULL, msg);
 
@@ -89,6 +86,7 @@ int main(int argc, char *argv[])
   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:      " << 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;
 }
diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp
index 856755d1c10f46f8aa15b852c48cefd10d8cc2f4..6417b51cda0fbbc3aef3535496bed5020f23c4a0 100644
--- a/src/gnss_utils.cpp
+++ b/src/gnss_utils.cpp
@@ -12,44 +12,41 @@ ComputePosOutput computePos(const GNSSUtils::Observations& _observations,
   // Define error msg
   char msg[128] = "";
 
-    // Remove duplicated satellites
-    _navigation.uniqueNavigation();
-
-    GNSSUtils::ComputePosOutput output;
-    sol_t sol;
-    sol = {{0}};
-
-    output.pos_stat = pntpos(_observations.data(), _observations.size(),
-                            &(_navigation.getNavigation()),
-                            &_prcopt, &sol, NULL, NULL, msg);
-    
-    if (output.pos_stat == 0)
-    {
-      std::cout << "computePos: error in computing positioning, message: "  << msg << "\n";
-    }
-
-    output.time = sol.time.time;
-    output.sec = sol.time.sec;
-    output.pos  = Eigen::Vector3d(sol.rr);
-    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;
+  // Remove duplicated satellites
+  _navigation.uniqueNavigation();
+
+  GNSSUtils::ComputePosOutput output;
+  sol_t                       sol;
+  sol = { { 0 } };
+
+  output.pos_stat = pntpos(
+      _observations.data(), _observations.size(), &(_navigation.getNavigation()), &_prcopt, &sol, NULL, NULL, msg);
+
+  if (output.pos_stat == 0)
+  {
+    std::cout << "computePos: error in computing positioning, message: " << msg << "\n";
+  }
+
+  output.time = sol.time.time;
+  output.sec  = sol.time.sec;
+  output.pos  = Eigen::Vector3d(sol.rr);
+  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;
 }
 
 // ComputePosOutput computePosOwn(const GNSSUtils::Observations & _observations,
diff --git a/src/navigation.cpp b/src/navigation.cpp
index a3e3dac38d96b00f539d712b76e5f9f8465dba14..a1ccfd7a04671fb7f69f9c4174b374b60a6b116e 100644
--- a/src/navigation.cpp
+++ b/src/navigation.cpp
@@ -4,33 +4,35 @@ using namespace GNSSUtils;
 
 Navigation::Navigation()
 {
-    // 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();
+  // 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(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();
-    setNavigation(nav.getNavigation());
+  // 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();
+  setNavigation(nav.getNavigation());
 }
 
 Navigation::~Navigation()
@@ -40,225 +42,224 @@ Navigation::~Navigation()
 
 void Navigation::setNavigation(nav_t _nav)
 {
-    freeNavigationArrays();
-
-    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) */
+  freeNavigationArrays();
 
-    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) */
+  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) */
 
-    copySbasCorrections(_nav);
-    //    sbssat_t sbssat;    /* SBAS satellite corrections */
-    //    sbsion_t sbsion[MAXBAND+1]; /* SBAS ionosphere corrections */
+  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 */
 
-    // ********** 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 */
+  // ********** 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::clearNavigation()
 {
-    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;
+  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::loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt, const char* opt)
 {
-    auto stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, NULL, &nav_, NULL);
-    if (stat == 0)
-    {
-        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 << "Couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error") << std::endl;
+  auto stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, NULL, &nav_, NULL);
+  if (stat == 0)
+  {
+    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 << "Couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error") << std::endl;
 }
 
 void Navigation::copyAllArrays(const nav_t& nav)
 {
-    //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);
+  // 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();
+  uniqueNavigation();
 }
 
 void Navigation::copyEphemeris(const nav_t& nav)
 {
-    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]);
-//    }
-//  }
+  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]);
+  //    }
+  //  }
 
-    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<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]);
-//    }
-//  }
+  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::copyAlmanac(const nav_t& nav)
 {
-    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]);
-//    }
-//  }
+  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::copyIonUtc(const nav_t &nav)
+void Navigation::copyIonUtc(const nav_t& nav)
 {
-    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);
+  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);
 
-    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);
+  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;
+  nav_.leaps = nav.leaps;
 }
 
-void Navigation::copySbasCorrections(const nav_t &nav)
+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);
-    }
+  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::freeNavigationArrays()
 {
-    // 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();
+  // 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()
@@ -271,23 +272,32 @@ void Navigation::print()
   std::cout << "na: " << nav_.na << "\n";
   std::cout << "nt: " << nav_.nt << "\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";
+  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 1b39857bbe0252fdf45491c6ebba405c42c8c051..98b72a6ad523e15833219def37c160163802e14f 100644
--- a/src/observations.cpp
+++ b/src/observations.cpp
@@ -1,38 +1,33 @@
 #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())
+  : 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);
-    }
+  // 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()
@@ -46,116 +41,123 @@ void Observations::clearObservations()
   this->sat_2_idx_.clear();
 }
 
-void Observations::addObservation(const obsd_t & obs)
+void Observations::addObservation(const obsd_t& 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());
+  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)
+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 << "Couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error") << std::endl;
-        return;
-    }
+  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 << "Couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error") << 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]);
-    }
+  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);
+  free(obs.data);
 }
 
 void Observations::removeObservationByIdx(const int& _idx)
 {
-    //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());
+  // 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());
 }
 
 void Observations::removeObservationBySat(const int& _sat)
 {
-    //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());
+  // 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);
@@ -186,33 +188,34 @@ void Observations::print()
   }
 }
 
-void Observations::findCommonObservations(const Observations& obs_1, const Observations& obs_2,
-                                          Observations& common_obs_1, Observations& common_obs_2)
+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;
+  // 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
index 8fdc8cbfa8f09ff7b71f65a5c245c8edeb1cf297..9cc2442d19ddaefe97e7741878a21a565fffbb50 100644
--- a/src/ublox_raw.cpp
+++ b/src/ublox_raw.cpp
@@ -2,98 +2,97 @@
 
 using namespace GNSSUtils;
 
-UBloxRaw::UBloxRaw() :
-  raw_data_type_(NO)
+UBloxRaw::UBloxRaw() : raw_data_type_(NO)
 {
-    if (init_raw(&raw_data_, STRFMT_UBX) == 0)
-    {
-        assert("Failed when allocating memory for raw_t");
-        return;
-    }
+  if (init_raw(&raw_data_, STRFMT_UBX) == 0)
+  {
+    assert("Failed when allocating memory for raw_t");
+    return;
+  }
 };
 
 UBloxRaw::~UBloxRaw()
 {
-    free_raw(&raw_data_);
+  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);
+  // 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;
+    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 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_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_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;
+      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;
-        }
+      // 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_;
+  return raw_data_type_;
 }
 
 void UBloxRaw::updateObservations()
 {
-    // sort and remove duplicated observations
-    sortobs(&raw_data_.obs);
-    // std::cout << "---------------------------JUST BEFORE!-------------------" << std::endl;
-    // obs_.print();
+  // 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]);
+  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();
+  // std::cout << "--------------------------JUST AFTER!---------------------" << std::endl;
+  // obs_.print();
 }
diff --git a/src/utils.cpp b/src/utils.cpp
index c967e3f7c26ea944279401f68777fb99b486a789..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 << (int)(_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 << ",";
   }
-
 }
+
+}  // namespace GNSSUtils