From 9a3a5e295f11a7aefdfae7438734800f86ef17a7 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 10 Apr 2020 11:41:02 +0200
Subject: [PATCH] changed some API and reimplemented tdcp

---
 include/gnss_utils/observations.h    |  50 +-
 include/gnss_utils/snapshot.h        |  23 +-
 include/gnss_utils/tdcp.h            |  65 +-
 include/gnss_utils/ublox_raw.h       |   4 +-
 include/gnss_utils/utils/satellite.h |  11 +-
 src/observations.cpp                 | 265 +++++---
 src/tdcp.cpp                         | 916 ++++++++++++---------------
 src/utils/satellite.cpp              |   5 +-
 test/gtest_observations.cpp          |  67 +-
 test/gtest_transformations.cpp       |   1 +
 10 files changed, 705 insertions(+), 702 deletions(-)

diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h
index a4056d0..caba378 100644
--- a/include/gnss_utils/observations.h
+++ b/include/gnss_utils/observations.h
@@ -11,7 +11,6 @@
 #include <eigen3/Eigen/Dense>
 
 #include "gnss_utils/gnss_utils.h"
-#include "gnss_utils/utils/utils.h"
 
 namespace GnssUtils
 {
@@ -19,6 +18,8 @@ class Observations;
 typedef std::shared_ptr<Observations>       ObservationsPtr;
 typedef std::shared_ptr<const Observations> ObservationsConstPtr;
 
+typedef std::map<int,Eigen::Vector3d> SatellitesPositions;
+
 class Observations
 {
 public:
@@ -63,25 +64,25 @@ public:
   void printByIdx(const int& _idx);
   void print();
 
-  // TODO:
-//  std::set<int> filterEphemeris() const;
-//  std::set<int> removeSatellites() const;
-//  std::set<int> filterWrongCode() const;
-//  std::set<int> filterWrongCarrierPhase() const;
-//  std::set<int> filterConstellations() const;
-//  std::set<int> filterElevation() const;
-//  std::set<int> filterSnr() const;
-  std::set<int> filter(std::map<int, Eigen::Vector3d>& sats_pos,
-                       std::set<int>&                  discarded_sats,
-                       const Eigen::Vector3d&          x_r,
-                       const bool&                     check_code,
-                       const bool&                     check_carrier_phase,
-                       const prcopt_t&                 opt);
-
-  static void findCommonObservations(const Observations& obs_1,
-                                     const Observations& obs_2,
-                                     Observations&       common_obs_1,
-                                     Observations&       common_obs_2);
+  // Filter observations
+  std::set<int> filterByEphemeris(const SatellitesPositions& sats_pos);
+  std::set<int> filterBySatellites(const std::set<int>& discarded_sats);
+  std::set<int> filterByCode();
+  std::set<int> filterByCarrierPhase();
+  std::set<int> filterByConstellations(const int& navsys);
+  std::set<int> filterByElevationSnr(const Eigen::Vector3d& x_r,
+                                     const SatellitesPositions& sats_pos,
+                                     const snrmask_t& snrmask,
+                                     const double& elmin);
+  std::set<int> filter(const SatellitesPositions&   sats_pos,
+                       const std::set<int>&         discarded_sats,
+                       const Eigen::Vector3d&       x_r,
+                       const bool&                  check_code,
+                       const bool&                  check_carrier_phase,
+                       const prcopt_t&              opt);
+
+  static std::set<int> findCommonObservations(const Observations& obs_1,
+                                              const Observations& obs_2);
 
   bool operator==(const Observations& other_obs) const;
   bool operator !=(const Observations &other_obs) const;
@@ -95,6 +96,15 @@ private:
   // Private methods
 };
 
+}  // namespace GnssUtils
+
+// IMPLEMENTATION
+#include "gnss_utils/utils/utils.h"
+#include "gnss_utils/utils/satellite.h"
+
+namespace GnssUtils
+{
+
 inline const std::vector<obsd_t>& Observations::getObservations() const
 {
   return this->obs_;
diff --git a/include/gnss_utils/snapshot.h b/include/gnss_utils/snapshot.h
index 41c8dfc..8ba30db 100644
--- a/include/gnss_utils/snapshot.h
+++ b/include/gnss_utils/snapshot.h
@@ -9,16 +9,22 @@
 
 #include <eigen3/Eigen/Dense>
 
-#include "gnss_utils/observations.h"
-#include "gnss_utils/navigation.h"
+#include "gnss_utils/gnss_utils.h"
 
 namespace GnssUtils
 {
+
+// forward declarations
+class Observations;
+class Navigation;
+typedef std::map<int,Eigen::Vector3d> SatellitesPositions;
+typedef std::shared_ptr<Observations>   ObservationsPtr;
+typedef std::shared_ptr<Navigation>     NavigationPtr;
+
 class Snapshot;
 typedef std::shared_ptr<Snapshot>       SnapshotPtr;
 typedef std::shared_ptr<const Snapshot> SnapshotConstPtr;
 
-typedef std::map<int,Eigen::Vector3d> SatellitesPositions;
 
 class Snapshot
 {
@@ -60,6 +66,17 @@ private:
   // Private methods
 };
 
+}  // namespace GnssUtils
+
+// IMPLEMENTATION
+#include "gnss_utils/utils/satellite.h"
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
+#include "gnss_utils/utils/utils.h"
+
+namespace GnssUtils
+{
+
 inline GnssUtils::ObservationsPtr Snapshot::getObservations() const
 {
     return obs_;
diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
index 0a0b2ff..10e280d 100644
--- a/include/gnss_utils/tdcp.h
+++ b/include/gnss_utils/tdcp.h
@@ -13,8 +13,7 @@
 #include <set>
 #include "gnss_utils/utils/rcv_position.h"
 #include "utils/satellite.h"
-#include "gnss_utils/observations.h"
-#include "gnss_utils/navigation.h"
+#include "gnss_utils/snapshot.h"
 
 namespace GnssUtils
 {
@@ -36,52 +35,36 @@ struct TdcpParams
   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(SnapshotPtr           snapshot_r,
+          SnapshotPtr           snapshot_k,
+          Eigen::Vector4d&      d,
+          Eigen::Matrix4d&      cov_d,
+          double&               residual,
+          const std::set<int>&  discarded_sats,
+          std::set<int>&        raim_discarded_sats,
+          const TdcpParams&     sd_opt,
+          const prcopt_t&       opt);
 
-bool Tdcp(const Observations&    obs_r,
-          const Navigation&      nav_r,
+bool Tdcp(SnapshotPtr            snapshot_r,
+          SnapshotPtr            snapshot_k,
           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 std::set<int>&   discarded_sats,
+          std::set<int>&         raim_discarded_sats,
+          const TdcpParams&      sd_params,
           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 filterCommonObservations(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 bool&                     check_code,
-                                       const bool&                     check_carrier_phase,
-                                       const prcopt_t&                 opt);
-
+bool Tdcp(SnapshotPtr               snapshot_r,
+          SnapshotPtr               snapshot_k,
+          const Eigen::Vector3d&    x_r,
+          const std::set<int>       common_sats,
+          Eigen::Vector4d&          d,
+          Eigen::Matrix4d&          cov_d,
+          double&                   residual,
+          std::set<int>             raim_discarded_sats,
+          const TdcpParams&         sd_params);
 }  // namespace GnssUtils
 
 #endif /* INCLUDE_GNSS_UTILS_Tdcp_H_ */
diff --git a/include/gnss_utils/ublox_raw.h b/include/gnss_utils/ublox_raw.h
index ecee80c..f03bb73 100644
--- a/include/gnss_utils/ublox_raw.h
+++ b/include/gnss_utils/ublox_raw.h
@@ -44,12 +44,12 @@ private:
   void updateObservations();
 };
 
-inline const GnssUtils::Observations& UBloxRaw::getObservations()
+inline const Observations& UBloxRaw::getObservations()
 {
   return obs_;
 }
 
-inline const GnssUtils::Navigation& UBloxRaw::getNavigation()
+inline const Navigation& UBloxRaw::getNavigation()
 {
   return nav_;
 }
diff --git a/include/gnss_utils/utils/satellite.h b/include/gnss_utils/utils/satellite.h
index 665443d..8ca85f3 100644
--- a/include/gnss_utils/utils/satellite.h
+++ b/include/gnss_utils/utils/satellite.h
@@ -12,14 +12,17 @@
 #include <eigen3/Eigen/Geometry>
 
 #include <set>
+#include <map>
 
-#include "gnss_utils/observations.h"
-#include "gnss_utils/navigation.h"
-#include "gnss_utils/utils/transformations.h"
-#include "gnss_utils/gnss_utils.h"
 
 namespace GnssUtils
 {
+    // forward declarations
+    class Observations;
+    class Navigation;
+
+    typedef std::map<int,Eigen::Vector3d> SatellitesPositions;
+
     double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef);
 
     void computeSatellitesPositions(const Observations&             obs,
diff --git a/src/observations.cpp b/src/observations.cpp
index 4c455d0..2cedfe3 100644
--- a/src/observations.cpp
+++ b/src/observations.cpp
@@ -188,108 +188,216 @@ void Observations::print()
   }
 }
 
-std::set<int> Observations::filter(std::map<int, Eigen::Vector3d>& sats_pos,
-                                   std::set<int>&                  discarded_sats,
-                                   const Eigen::Vector3d&          x_r,
-                                   const bool&                     check_code,
-                                   const bool&                     check_carrier_phase,
-                                   const prcopt_t&                 opt)
+std::set<int> Observations::filterByEphemeris(const SatellitesPositions& sats_pos)
 {
-  std::cout << "filterSatellites: initial size: " << obs_.size() << std::endl;
+    std::set<int> remove_sats;
 
-  std::set<int> remove_sats;
+    for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
+    {
+      auto&& obs_sat = getObservationByIdx(obs_i);
+      const int& sat_number = obs_sat.sat;
+
+      // bad or inexistent satellite position (satellite is not included in the discarded list)
+      if (sats_pos.count(sat_number) == 0 or
+          sats_pos.at(sat_number).isApprox(Eigen::Vector3d::Zero(), 1e-3) or
+          sats_pos.at(sat_number).isApprox(Eigen::Vector3d::Zero(), 1e-3))
+      {
+        std::cout << "Discarding sat " << sat_number << ": wrong satellite position: \n\t" << sats_pos.at(sat_number).transpose() << std::endl;
+        remove_sats.insert(sat_number);
+      }
+    }
 
-  for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
-  {
-    auto&& obs_sat = getObservationByIdx(obs_i);
-    const int& sat_number = obs_sat.sat;
+    // remove sats
+    // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+    for (auto sat : remove_sats)
+    {
+      assert(hasSatellite(sat));
+      removeObservationBySat(sat);
+    }
+
+    return remove_sats;
+}
+
+std::set<int> Observations::filterBySatellites(const std::set<int>& discarded_sats)
+{
+    std::set<int> remove_sats;
 
-    // already discarded sats
-    if (discarded_sats.count(sat_number) != 0)
+    for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
     {
-      remove_sats.insert(sat_number);
-      continue;
+      auto&& obs_sat = getObservationByIdx(obs_i);
+      const int& sat_number = obs_sat.sat;
+
+      // discard sats
+      if (discarded_sats.count(sat_number) != 0)
+        remove_sats.insert(sat_number);
     }
 
-    // wrong data (satellite is not included in the discarded list)
-    if (check_carrier_phase and std::abs(obs_sat.L[0]) < 1e-12)
+    // remove sats
+    // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+    for (auto sat : remove_sats)
     {
-      std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_sat.L[0] << std::endl;
-      remove_sats.insert(sat_number);
-      continue;
+      assert(hasSatellite(sat));
+      removeObservationBySat(sat);
     }
-    if (check_code and std::abs(obs_sat.P[0]) < 1e-12)
+
+    return remove_sats;
+}
+
+std::set<int> Observations::filterByCode()
+{
+    std::set<int> remove_sats;
+
+    for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
+    {
+      auto&& obs_sat = getObservationByIdx(obs_i);
+      const int& sat_number = obs_sat.sat;
+
+      // wrong data
+      if (std::abs(obs_sat.P[0]) < 1e-12)
+      {
+        std::cout << "Discarding sat " << sat_number << ": wrong code data: " << obs_sat.P[0] << std::endl;
+        remove_sats.insert(sat_number);
+      }
+    }
+
+    // remove sats
+    // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+    for (auto sat : remove_sats)
+    {
+      assert(hasSatellite(sat));
+      removeObservationBySat(sat);
+    }
+
+    return remove_sats;
+}
+
+std::set<int> Observations::filterByCarrierPhase()
+{
+    std::set<int> remove_sats;
+
+    for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
     {
-      std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_sat.P[0] << std::endl;
-      remove_sats.insert(sat_number);
-      continue;
+      auto&& obs_sat = getObservationByIdx(obs_i);
+      const int& sat_number = obs_sat.sat;
+
+      // wrong data
+      if (std::abs(obs_sat.L[0]) < 1e-12)
+      {
+        std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_sat.L[0] << std::endl;
+        remove_sats.insert(sat_number);
+      }
     }
 
-    // bad or inexistent satellite position (satellite is not included in the discarded list)
-    if (sats_pos.count(sat_number) == 0 or
-        sats_pos[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3) or
-        sats_pos[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3))
+    // remove sats
+    // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+    for (auto sat : remove_sats)
     {
-      std::cout << "Discarding sat " << sat_number << ": wrong satellite position: \n\t" << sats_pos[sat_number].transpose() << std::endl;
-      remove_sats.insert(sat_number);
-      continue;
+      assert(hasSatellite(sat));
+      removeObservationBySat(sat);
     }
 
-    // constellation
-    int sys = satsys(obs_sat.sat, NULL);
-    if (!(sys & opt.navsys))
+    return remove_sats;
+}
+
+std::set<int> Observations::filterByConstellations(const int& navsys)
+{
+    std::set<int> remove_sats;
+
+    for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
     {
-      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;
+      auto&& obs_sat = getObservationByIdx(obs_i);
+      const int& sat_number = obs_sat.sat;
+
+      // constellation
+      int sys = satsys(obs_sat.sat, NULL);
+      if (!(sys & navsys))
+      {
+        std::cout << "Discarding sat " << sat_number << ": not selected constellation " << sys << " - mask: " << navsys << std::endl;
+        remove_sats.insert(sat_number);
+        continue;
+      }
     }
 
-    // check both elevations
-    double elevation = computeSatElevation(x_r, sats_pos[sat_number]);
-    if (elevation < opt.elmin)
+    // remove sats
+    // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+    for (auto sat : remove_sats)
     {
-      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;
+      assert(hasSatellite(sat));
+      removeObservationBySat(sat);
     }
 
-    // snr TODO: multifrequency (2nd param and 3rd idx)
-    if (testsnr(0, 0, elevation, obs_sat.SNR[0] * 0.25, &opt.snrmask) == 1)
+    return remove_sats;
+}
+
+std::set<int> Observations::filterByElevationSnr(const Eigen::Vector3d& x_r,
+                                                 const SatellitesPositions& sats_pos,
+                                                 const snrmask_t& snrmask,
+                                                 const double& elmin)
+{
+    std::set<int> remove_sats;
+
+    for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
     {
-      std::cout << "Discarding sat " << sat_number << ": snr test " << std::endl;
-      discarded_sats.insert(sat_number);
-      remove_sats.insert(sat_number);
-      continue;
+      auto&& obs_sat = getObservationByIdx(obs_i);
+      const int& sat_number = obs_sat.sat;
+
+      // check elevation
+      double elevation = computeSatElevation(x_r, sats_pos.at(sat_number));
+      if (elevation < elmin)
+      {
+        std::cout << "Discarding sat " << sat_number << ": low elevation " << elevation << " - min: " << elmin << std::endl;
+        remove_sats.insert(sat_number);
+        continue;
+      }
+
+      // snr TODO: multifrequency (2nd param and 3rd idx)
+      if (testsnr(0, 0, elevation, obs_sat.SNR[0] * 0.25, &snrmask) == 1)
+      {
+        std::cout << "Discarding sat " << sat_number << ": snr test " << std::endl;
+        remove_sats.insert(sat_number);
+      }
     }
-  }
 
-  // remove sats
-  // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
-  for (auto sat : remove_sats)
-  {
-    assert(hasSatellite(sat));
-    assert(sats_pos.count(sat));
-    removeObservationBySat(sat);
-    sats_pos.erase(sat);
-  }
+    // remove sats
+    // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+    for (auto sat : remove_sats)
+    {
+      assert(hasSatellite(sat));
+      removeObservationBySat(sat);
+    }
 
-  assert(obs_.size() == sats_pos.size());
+    return remove_sats;
+}
 
-  return remove_sats;
+std::set<int> Observations::filter(const SatellitesPositions&   sats_pos,
+                                   const std::set<int>&         discarded_sats,
+                                   const Eigen::Vector3d&       x_r,
+                                   const bool&                  check_code,
+                                   const bool&                  check_carrier_phase,
+                                   const prcopt_t&              opt)
+{
+  //std::cout << "filter: initial size: " << obs_.size() << std::endl;
+
+  std::set<int> remove_sats_1 = filterByEphemeris(sats_pos);
+  std::set<int> remove_sats_2 = filterBySatellites(discarded_sats);
+  std::set<int> remove_sats_3 = filterByCode();
+  std::set<int> remove_sats_4 = filterByCarrierPhase();
+  std::set<int> remove_sats_5 = filterByConstellations(opt.navsys);
+  std::set<int> remove_sats_6 = filterByElevationSnr(x_r, sats_pos, opt.snrmask, opt.elmin);
+
+  remove_sats_1.insert(remove_sats_2.begin(), remove_sats_2.end());
+  remove_sats_1.insert(remove_sats_3.begin(), remove_sats_3.end());
+  remove_sats_1.insert(remove_sats_4.begin(), remove_sats_4.end());
+  remove_sats_1.insert(remove_sats_5.begin(), remove_sats_5.end());
+  remove_sats_1.insert(remove_sats_6.begin(), remove_sats_6.end());
+
+  return remove_sats_1;
   // std::cout << "final size: " << obs_.size() << std::endl;
 }
 
-void Observations::findCommonObservations(const Observations& obs_1,
-                                          const Observations& obs_2,
-                                          Observations&       common_obs_1,
-                                          Observations&       common_obs_2)
+std::set<int> Observations::findCommonObservations(const Observations& obs_1,
+                                                   const Observations& 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 << " ";
@@ -299,18 +407,19 @@ void Observations::findCommonObservations(const Observations& obs_1,
   //    std::cout << (int)obs_2_ref.sat << " ";
   // std::cout << std::endl;
 
+  std::set<int> common_sats;
+
   // 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));
-    }
+      common_sats.insert(obs_1_ref.sat);
 
   // std::cout << "common sats: ";
-  // for (auto&& obs_2_ref : common_obs_1.getObservations())
-  //    std::cout << (int)obs_2_ref.sat << " ";
+  // for (auto sat : common_sats)
+  //    std::cout << sat << " ";
   // std::cout << std::endl;
+
+  return common_sats;
 }
 
 
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index a3d6d35..cfe42cc 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -9,570 +9,466 @@
 
 namespace GnssUtils
 {
-bool Tdcp(const Observations& obs_r,
-          Navigation&         nav_r,
-          const Observations& obs_k,
-          const Navigation&   nav_k,
+bool Tdcp(SnapshotPtr snapshot_r,
+          SnapshotPtr snapshot_k,
           Eigen::Vector4d&    d,
           Eigen::Matrix4d&    cov_d,
           double&             residual,
-          std::set<int>&      discarded_sats,
+          const std::set<int>&   discarded_sats,
+          std::set<int>&         raim_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);
+    return Tdcp(snapshot_r,
+                snapshot_k,
+                computePos(*snapshot_r->getObservations(), *snapshot_r->getNavigation(), opt).pos,
+                d,
+                cov_d,
+                residual,
+                discarded_sats,
+                raim_discarded_sats,
+                sd_params,
+                opt);
 }
 
-bool Tdcp(const Observations&    obs_r,
-          const Navigation&      nav_r,
+bool Tdcp(SnapshotPtr            snapshot_r,
+          SnapshotPtr            snapshot_k,
           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 std::set<int>&   discarded_sats,
+          std::set<int>&         raim_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.sateph, common_sats_pos_r);
-  computeSatellitesPositions(common_obs_k, sd_params.use_old_nav ? nav_r : nav_k, opt.sateph, common_sats_pos_k);
-
-  // FILTER SATELLITES (ALREADY DISCARDED, CORRUPTED DATA, WRONG POSITION, CONSTELLATION, ELEVATION and SNR)
-  filterCommonObservations(
-      common_obs_r, common_sats_pos_r, common_obs_k, common_sats_pos_k, discarded_sats, x_r, !sd_params.use_carrier_phase, sd_params.use_carrier_phase, opt);
-
-  // COMPUTE SINGLE DIFFERENCES
-  return Tdcp(common_obs_r,
-              nav_r,
-              common_sats_pos_r,
-              x_r,
-              common_obs_k,
-              nav_k,
-              common_sats_pos_k,
-              d,
-              cov_d,
-              residual,
-              discarded_sats,
-              sd_params);
-}
-
-bool Tdcp(const Observations&                   common_obs_r,
-          const Navigation&                     nav_r,
-          const std::map<int, Eigen::Vector3d>& common_sats_pos_r,
-          const Eigen::Vector3d&                x_r,
-          const Observations&                   common_obs_k,
-          const Navigation&                     nav_k,
-          const std::map<int, Eigen::Vector3d>& common_sats_pos_k,
-          Eigen::Vector4d&                      d,
-          Eigen::Matrix4d&                      cov_d,
-          double&                               residual,
-          std::set<int>&                        discarded_sats,
-          const TdcpParams&                     sd_params)
-{
-  assert(common_obs_r.size() == common_obs_k.size());
-  assert(common_obs_r.size() == common_sats_pos_r.size());
-  assert(common_obs_k.size() == common_sats_pos_k.size());
-
-  double tr(common_obs_r.getObservations().front().time.time + common_obs_r.getObservations().front().time.sec);
-  double tk(common_obs_k.getObservations().front().time.time + common_obs_k.getObservations().front().time.sec);
-
-  int required_n_sats(std::max(sd_params.min_common_sats + sd_params.raim_n, 4 + sd_params.raim_n));
-  int n_common_sats = common_sats_pos_k.size();
-
-  if (n_common_sats < required_n_sats)
-  {
-#if GNSS_UTILS_TDCP_DEBUG == 1
-    printf("Tdcp: NOT ENOUGH COMMON SATS");
-    printf("Tdcp: %i common sats available, %i sats are REQUIRED. [ SKIPPING Tdcp ]", n_common_sats, required_n_sats);
-#endif
-    return false;
-  }
-
-  // MULTI-FREQUENCY
-  std::map<int, std::pair<int, int>> row_2_sat_freq;
-  int                                row = 0;
-  for (auto obs_idx = 0; obs_idx < common_obs_k.size(); obs_idx++)
-  {
-    auto&& obs_r = common_obs_r.getObservationByIdx(obs_idx);
-    auto&& obs_k = common_obs_k.getObservationByIdx(obs_idx);
-
-    const int& sat_number = obs_k.sat;
-
-    if (sd_params.use_carrier_phase)
+    // If use old nav temporary change navigation to (re)compute satellites positions
+    auto nav_k = snapshot_k->getNavigation();
+    if (sd_params.use_old_nav)
     {
-      // 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++;
-      }
+        snapshot_k->getSatellitesPositions().clear();
+        snapshot_k->setNavigation(snapshot_r->getNavigation());
     }
-    else
-        // L1/E1
-        if (std::abs(obs_r.P[0]) > 1e-12 and std::abs(obs_k.P[0]) > 1e-12)
+
+    // COMPUTE SATELLITES POSITION
+    if (!snapshot_r->satellitesPositionsComputed())
+        snapshot_r->computeSatellitesPositions(opt.sateph);
+    if (!snapshot_k->satellitesPositionsComputed())
+        snapshot_k->computeSatellitesPositions(opt.sateph);
+
+    // FILTER SATELLITES (ALREADY DISCARDED, CORRUPTED DATA, WRONG POSITION, CONSTELLATION, ELEVATION and SNR)
+    snapshot_r->getObservations()->filter(snapshot_r->getSatellitesPositions(),
+                                          discarded_sats,
+                                          x_r,
+                                          !sd_params.use_carrier_phase,
+                                          sd_params.use_carrier_phase,
+                                          opt);
+    snapshot_k->getObservations()->filter(snapshot_k->getSatellitesPositions(),
+                                          discarded_sats,
+                                          x_r,
+                                          !sd_params.use_carrier_phase,
+                                          sd_params.use_carrier_phase,
+                                          opt);
+
+    // FIND COMMON SATELLITES OBSERVATIONS
+    std::set<int> common_sats = Observations::findCommonObservations(*snapshot_r->getObservations(),
+                                                                     *snapshot_k->getObservations());
+
+    // COMPUTE TDCP
+    bool tdcp_ok = Tdcp(snapshot_r,
+                        snapshot_k,
+                        x_r,
+                        common_sats,
+                        d,
+                        cov_d,
+                        residual,
+                        raim_discarded_sats,
+                        sd_params);
+
+    // UNDO temporary change navigation
+    if (sd_params.use_old_nav)
     {
-      row_2_sat_freq[row] = std::make_pair(sat_number, 0);
-      row++;
+        snapshot_k->setNavigation(nav_k);
+        snapshot_k->computeSatellitesPositions(opt.sateph);
     }
-  }
-  int n_differences = row_2_sat_freq.size();
-
-  // Initial guess
-  Eigen::Vector4d d_0(d), delta_d(Eigen::Vector4d::Zero());
-
-#if GNSS_UTILS_TDCP_DEBUG == 1
-  std::cout << "Receiver at t_r: " << x_r.transpose() << std::endl;
-  std::cout << "d initial guess: " << d_0.transpose() << std::endl;
-  std::cout << "common sats: ";
-  for (auto row_sat_freq_it : row_2_sat_freq)
-    std::cout << row_sat_freq_it.second.first << " ";
-  std::cout << std::endl;
-  std::cout << "discarded_sats: ";
-  for (auto sat : discarded_sats)
-    std::cout << sat << " ";
-  std::cout << std::endl;
-#endif
-
-  // FILL SATELLITES POSITIONS AND MEASUREMENTS =======================================================================
-  Eigen::Matrix<double, Eigen::Dynamic, 4> A(Eigen::Matrix<double, Eigen::Dynamic, 4>::Zero(n_differences, 4));
-  Eigen::VectorXd                          r(Eigen::VectorXd::Zero(n_differences));
-  Eigen::VectorXd                          drho_m(Eigen::VectorXd::Zero(n_differences));
-  Eigen::Matrix<double, 3, Eigen::Dynamic> s_k(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3, n_differences));
-  Eigen::Matrix<double, 3, Eigen::Dynamic> s_r(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3, n_differences));
-  int                                      sat_i = 0;
-
-  for (auto row_sat_freq_it : row_2_sat_freq)
-  {
-    const int& row        = row_sat_freq_it.first;
-    const int& sat_number = row_sat_freq_it.second.first;
-    const int& freq       = row_sat_freq_it.second.second;
-
-    auto obs_r = common_obs_r.getObservationBySat(sat_number);
-    auto obs_k = common_obs_k.getObservationBySat(sat_number);
-
-    // excluded satellite
-    if (discarded_sats.count(sat_number) != 0)
+
+    return tdcp_ok;
+}
+
+bool Tdcp(SnapshotPtr               snapshot_r,
+          SnapshotPtr               snapshot_k,
+          const Eigen::Vector3d&    x_r,
+          const std::set<int>       common_sats,
+          Eigen::Vector4d&          d,
+          Eigen::Matrix4d&          cov_d,
+          double&                   residual,
+          std::set<int>             raim_discarded_sats,
+          const TdcpParams&         sd_params)
+{
+    // Checks
+    assert(snapshot_r->satellitesPositionsComputed() && "this TDCP assumes satellites already computed");
+    assert(snapshot_k->satellitesPositionsComputed() && "this TDCP assumes satellites already computed");
+    assert(snapshot_r->getSatellitesPositions().size() > snapshot_r->getObservations()->size() && "Satellites without position not filtered");
+    assert(snapshot_k->getSatellitesPositions().size() > snapshot_k->getObservations()->size() && "Satellites without position not filtered");
+    assert(snapshot_r->getSatellitesPositions().size() > common_sats.size() && "Too many common sats");
+    assert(snapshot_k->getSatellitesPositions().size() > common_sats.size() && "Too many common sats");
+    assert(raim_discarded_sats.empty() && "just output param");
+
+    for (auto sat : common_sats)
     {
-#if GNSS_UTILS_TDCP_DEBUG == 1
-      std::cout << "\tdiscarded sat" << std::endl;
-#endif
-      continue;
+        assert(snapshot_r->getObservations()->hasSatellite(sat) && "wrong common sat");
+        assert(snapshot_k->getObservations()->hasSatellite(sat) && "wrong common sat");
+        assert(snapshot_r->getSatellitesPositions().count(sat) && "common sat without position");
+        assert(snapshot_k->getSatellitesPositions().count(sat) && "common sat without position");
     }
 
-    // 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)
+    int n_common_sats = common_sats.size();
+    int required_n_sats(std::max(sd_params.min_common_sats + sd_params.raim_n, 4 + sd_params.raim_n));
+    if (n_common_sats < required_n_sats)
     {
-      // 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;
+        #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;
     }
-  }
 
-  // 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++)
+    // Times
+    double tr(-1), tk(-1);
+
+    // MULTI-FREQUENCY
+    std::map<int, std::pair<int, int>> row_2_sat_freq;
+    int                                row = 0;
+    for (auto sat : common_sats)
     {
-      // 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;
-      }
+        assert(snapshot_r->getObservations()->hasSatellite(sat));
+        assert(snapshot_k->getObservations()->hasSatellite(sat));
+
+        auto&& obs_r = snapshot_r->getObservations()->getObservationBySat(sat);
+        auto&& obs_k = snapshot_k->getObservations()->getObservationBySat(sat);
+
+        // fill times
+        if (tr < 0)
+        {
+            tr = obs_r.time.time + obs_r.time.sec;
+            tk = obs_k.time.time + obs_k.time.sec;
+        }
+
+        // Carrier phase
+        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, 0);
+
+            if (!sd_params.use_multi_freq)
+                continue;
+
+            // L2 (GPS/GLO/QZS)
+            int sys = satsys(sat, 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, 1);
+
+            // 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, 2);
+        }
+        // Code
+        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, 0);
     }
+    int n_differences = row_2_sat_freq.size();
+
+    // Initial guess
+    Eigen::Vector4d d_0(d), delta_d(Eigen::Vector4d::Zero());
+
+    #if GNSS_UTILS_TDCP_DEBUG == 1
+        std::cout << "Receiver at t_r: " << x_r.transpose() << std::endl;
+        std::cout << "d initial guess: " << d_0.transpose() << std::endl;
+        std::cout << "common sats: ";
+        for (auto row_sat_freq_it : row_2_sat_freq)
+            std::cout << row_sat_freq_it.second.first << " ";
+        std::cout << std::endl;
+    #endif
 
-    // Solve
-    cov_d   = (A.transpose() * A).inverse();
-    delta_d = -cov_d * A.transpose() * r;
-    d += delta_d;
+    // 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;
 
-    // wrong solution
-    if (d.array().isNaN().any())
+    for (auto row_sat_freq_it : row_2_sat_freq)
     {
-      std::cout << "Tdcp: LSM DID NOT WORK. NAN values appeared. ABORTING!!" << std::endl;
-      return false;
+        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 = snapshot_r->getObservations()->getObservationBySat(sat_number);
+        auto obs_k = snapshot_k->getObservations()->getObservationBySat(sat_number);
+        auto nav_r = snapshot_r->getNavigation()->getNavigation();
+        auto nav_k = snapshot_k->getNavigation()->getNavigation();
+
+        // Satellite's positions
+        s_r.col(row) = snapshot_r->getSatellitesPositions().at(sat_number);
+        s_k.col(row) = snapshot_r->getSatellitesPositions().at(sat_number);
+        nav_k.ion_gps;
+
+        if (sd_params.use_carrier_phase)  // TODO: add iono and tropo corrections (optionally)
+            drho_m(row) = (obs_k.L[freq] * nav_k.lam[sat_number - 1][freq]) -
+                          (obs_r.L[freq] * nav_r.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;
+        }
     }
 
-    // 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)
+    // LEAST SQUARES SOLUTION =======================================================================
+    for (int j = 0; j < sd_params.max_iterations; j++)
     {
-      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++)
+        // fill A and r
+        for (int row = 0; row < A.rows(); row++)
         {
-          int sat_removed = row_2_sat_freq.at(row_removed).first;
-
-          // Multi-freq: some rows for the same satellite
-          n_removed_rows = 1;
-          if (sd_params.use_multi_freq)
-            while (row_removed + n_removed_rows < A_raim.rows() and
-                   row_2_sat_freq.at(row_removed + n_removed_rows).first == sat_removed)
-              n_removed_rows++;
-
-          // remove satellite rows
-          for (auto r = 0; r < n_removed_rows; r++)
-          {
-            A_raim.row(row_removed + r) = Eigen::Vector4d::Zero().transpose();
-            r_raim(row_removed + r)     = 0;  // not necessary
-          }
-
-          // solve
-          Eigen::Vector4d delta_d_raim = -(A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim;
-          Eigen::Vector4d d_raim       = d_0 + delta_d_raim;
-
-          // no NaN
-          if (!d_raim.array().isNaN().any())
-          {
-            // residual
-            residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1);
-            // residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / A_raim.rows());
-
-#if GNSS_UTILS_TDCP_DEBUG == 1
-            std::cout << "RAIM excluding sat " << sat_removed << " - row " << row_removed << "(n_rows "
-                      << n_removed_rows << ")" << std::endl;
-            std::cout << "Displacement vector =" << d_raim.head<3>().transpose() << std::endl;
-            std::cout << "Displacement update =" << delta_d_raim.head<3>().transpose() << std::endl;
-            printf("Ref distance      = %7.3f m\n", d_0.norm());
-            printf("Computed distance = %7.3f m\n", d_raim.head<3>().norm());
-            printf("Tdcp: residual      = %13.10f\n", residual);
-            std::cout << "Tdcp: drho          = " << r_raim.transpose() << std::endl;
-            std::cout << "Tdcp: H             = \n" << A_raim << std::endl;
-            printf("Tdcp: dT            = %8.3f secs\n", d_raim(3));
-#endif
-            // store if best residual
-            if (residual < best_residual)
+            // 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)
             {
-              worst_sat_row = row_removed;
-              best_residual = residual;
-              best_d        = d_raim;
+                // 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;
             }
-          }
-          // 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)
+        // Solve
+        cov_d   = (A.transpose() * A).inverse();
+        delta_d = -cov_d * A.transpose() * r;
+        d += delta_d;
+
+        // wrong solution
+        if (d.array().isNaN().any())
         {
-          printf("Tdcp: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!");
-          return false;
+            std::cout << "Tdcp: LSM DID NOT WORK. NAN values appeared. ABORTING!!" << std::endl;
+            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
+        // 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
+        if (j == 0 and sd_params.raim_n > 0 and residual > sd_params.raim_min_residual)
         {
-          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);
-        }
+            int             worst_sat_row = -1;
+            double          best_residual = 1e12;
+            Eigen::Vector4d best_d;
+            int             n_removed_rows = 1;
 
-        // apply the RAIM solution
-        d     = best_d;
-        cov_d = (A.transpose() * A).inverse();
+            // remove some satellites
+            while (raim_discarded_sats.size() < sd_params.raim_n)
+            {
+                auto A_raim = A;
+                auto r_raim = r;
+
+                // solve removing each satellite
+                for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++)
+                {
+                    int sat_removed = row_2_sat_freq.at(row_removed).first;
+
+                    // Multi-freq: some rows for the same satellite
+                    n_removed_rows = 1;
+                    if (sd_params.use_multi_freq)
+                        while (row_removed + n_removed_rows < A_raim.rows() and
+                               row_2_sat_freq.at(row_removed + n_removed_rows).first == sat_removed)
+                            n_removed_rows++;
+
+                    // remove satellite rows
+                    for (auto r = 0; r < n_removed_rows; r++)
+                    {
+                        A_raim.row(row_removed + r) = Eigen::Vector4d::Zero().transpose();
+                        r_raim(row_removed + r)     = 0;  // not necessary
+                    }
+
+                    // solve
+                    Eigen::Vector4d delta_d_raim = -(A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim;
+                    Eigen::Vector4d d_raim       = d_0 + delta_d_raim;
+
+                    // no NaN
+                    if (!d_raim.array().isNaN().any())
+                    {
+                        // residual
+                        residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1);
+                        // residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / A_raim.rows());
+
+                        #if GNSS_UTILS_TDCP_DEBUG == 1
+                            std::cout << "RAIM excluding sat " << sat_removed << " - row " << row_removed << "(n_rows "
+                            << n_removed_rows << ")" << std::endl;
+                            std::cout << "Displacement vector =" << d_raim.head<3>().transpose() << std::endl;
+                            std::cout << "Displacement update =" << delta_d_raim.head<3>().transpose() << std::endl;
+                            printf("Ref distance      = %7.3f m\n", d_0.norm());
+                            printf("Computed distance = %7.3f m\n", d_raim.head<3>().norm());
+                            printf("Tdcp: residual      = %13.10f\n", residual);
+                            std::cout << "Tdcp: drho          = " << r_raim.transpose() << std::endl;
+                            std::cout << "Tdcp: H             = \n" << A_raim << std::endl;
+                            printf("Tdcp: dT            = %8.3f secs\n", d_raim(3));
+                        #endif
+
+                        // store if best residual
+                        if (residual < best_residual)
+                        {
+                            worst_sat_row = row_removed;
+                            best_residual = residual;
+                            best_d        = d_raim;
+                        }
+                    }
+                    // restore initial A and r
+                    for (auto row = 0; row < n_removed_rows; row++)
+                    {
+                        A_raim.row(row_removed + row) = A.row(row_removed + row);
+                        r_raim(row_removed + row)     = r(row_removed + row);
+                    }
+                    row_removed += (n_removed_rows - 1);
+                }
+
+                // No successful RAIM solution
+                if (worst_sat_row == -1)
+                {
+                    printf("Tdcp: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!");
+                    return false;
+                }
+
+                // store removed sat
+                raim_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 : ";
+                    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 : ";
+                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 iteration" << std::endl;
+        #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 << "\tCommon sats   : " << common_sats.size() << 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
+        std::cout << "\tResidual      = " << residual << std::endl;
+        #endif
     }
 
-#if GNSS_UTILS_TDCP_DEBUG == 1
-    std::cout << "Tdcp iteration " << j << std::endl;
-    std::cout << "\tExcluded sats : ";
-    for (auto dsat : discarded_sats)
-      std::cout << (int)dsat << " ";
-    std::cout << std::endl;
-    std::cout << "\tCommon sats   : " << n_common_sats << std::endl;
-    std::cout << "\tRows          : " << n_differences << std::endl;
-    std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
-    std::cout << "\tSolution      = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
-    std::cout << "\tClock offset  = " << d(3) << std::endl;
-    std::cout << "\tResidual      = " << residual << std::endl;
-#endif
-  }
-
-  // weight covariance with the measurement noise (proportional to time)
-  double sq_sigma_Tdcp = (tk - tr) * sd_params.sigma_atm * sd_params.sigma_atm +
-                         2 * (sd_params.use_carrier_phase ? sd_params.sigma_carrier * sd_params.sigma_carrier :
-                                                            sd_params.sigma_code * sd_params.sigma_code);
-  cov_d *= sq_sigma_Tdcp;
-  // residual = (r - A * d).norm() / A.rows();
-
-  // Computing error on measurement space
-  for (int row = 0; row < n_differences; row++)
-  {
-    r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);
-  }
-  residual = sqrt(r.squaredNorm() / r.size());
-
-  return true;
-}
-
-void filterCommonObservations(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 bool&                     check_code,
-                                const bool&                     check_carrier_phase,
-                                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());
+    // 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();
 
-  // TODO: 2 calls to filterObservations and take the common again
+    // 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);
 
-  std::cout << "filterSatellites: initial size: " << common_obs_k.size() << std::endl;
+    residual = sqrt(r.squaredNorm() / r.size());
 
-  std::set<int> remove_sats;
-
-  for (int obs_i = 0; obs_i < common_obs_r.size(); obs_i++)
-  {
-    auto&& obs_r = common_obs_r.getObservationByIdx(obs_i);
-    auto&& obs_k = common_obs_k.getObservationByIdx(obs_i);
-    assert(obs_r.sat == obs_k.sat && "common satellites observations have to be ordered equally");
-    const int& sat_number = obs_r.sat;
-
-    // already discarded sats
-    if (discarded_sats.count(sat_number) != 0)
-    {
-      remove_sats.insert(sat_number);
-      continue;
-    }
-
-    // wrong data (satellite is not included in the discarded list)
-    if (check_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 (check_code 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 << ": wrong satellite position: \n\t" << common_sats_pos_r[sat_number].transpose() << "\n\t" << common_sats_pos_k[sat_number].transpose() << std::endl;
-      remove_sats.insert(sat_number);
-      continue;
-    }
-
-    // constellation
-    int sys = satsys(obs_r.sat, NULL);
-    if (!(sys & opt.navsys))
-    {
-      std::cout << "Discarding sat " << sat_number << ": not selected constellation " << sys << " - mask: " << opt.navsys << std::endl;
-      discarded_sats.insert(sat_number);
-      remove_sats.insert(sat_number);
-      continue;
-    }
-
-    // check both elevations
-    double elevation = std::min(computeSatElevation(x_r, common_sats_pos_r[sat_number]),
-                                computeSatElevation(x_r, common_sats_pos_k[sat_number]));
-    if (elevation < opt.elmin)
-    {
-      std::cout << "Discarding sat " << sat_number << ": low elevation " << elevation << " - min: " << opt.elmin << std::endl;
-      discarded_sats.insert(sat_number);
-      remove_sats.insert(sat_number);
-      continue;
-    }
-
-    // snr TODO: multifrequency (2nd param and 3rd idx)
-    if (testsnr(0, 0, elevation, obs_r.SNR[0] * 0.25, &opt.snrmask) == 1)
-    {
-      std::cout << "Discarding sat " << sat_number << ": snr test " << std::endl;
-      discarded_sats.insert(sat_number);
-      remove_sats.insert(sat_number);
-      continue;
-    }
-  }
-
-  // remove sats
-  // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
-  for (auto sat : remove_sats)
-  {
-    assert(common_obs_r.hasSatellite(sat));
-    assert(common_obs_k.hasSatellite(sat));
-    assert(common_sats_pos_r.count(sat));
-    assert(common_sats_pos_k.count(sat));
-    common_obs_r.removeObservationBySat(sat);
-    common_obs_k.removeObservationBySat(sat);
-    common_sats_pos_r.erase(sat);
-    common_sats_pos_k.erase(sat);
-  }
-
-  assert(common_obs_r.size() == common_obs_k.size());
-  assert(common_obs_r.size() == common_sats_pos_r.size());
-  assert(common_obs_r.size() == common_sats_pos_k.size());
-
-  return remove_sats;
-  // std::cout << "final size: " << common_obs_k.size() << std::endl;
+    return true;
 }
 
 }  // namespace GnssUtils
diff --git a/src/utils/satellite.cpp b/src/utils/satellite.cpp
index 0a91930..878719e 100644
--- a/src/utils/satellite.cpp
+++ b/src/utils/satellite.cpp
@@ -7,7 +7,10 @@
 
 #include "gnss_utils/utils/satellite.h"
 
-using namespace GnssUtils;
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
+#include "gnss_utils/utils/transformations.h"
+#include "gnss_utils/gnss_utils.h"
 
 namespace GnssUtils
 {
diff --git a/test/gtest_observations.cpp b/test/gtest_observations.cpp
index 5d2de7a..d64de85 100644
--- a/test/gtest_observations.cpp
+++ b/test/gtest_observations.cpp
@@ -130,17 +130,17 @@ TEST(ObservationsTest, FindCommonObservations)
 
   ASSERT_TRUE(observations1 == observations2);
 
-  Observations common1;
-  Observations common2;
+  std::set<int> common_sats = Observations::findCommonObservations(observations1, observations2);
 
-  Observations::findCommonObservations(observations1, observations2, common1, common2);
-
-  ASSERT_TRUE(common1 == common2);
-  ASSERT_TRUE(observations1 == common1);
-  ASSERT_TRUE(observations2 == common2);
+  ASSERT_TRUE(common_sats.size() == observations1.size());
+  ASSERT_TRUE(common_sats.size() == observations2.size());
+  for (auto sat : common_sats)
+  {
+      ASSERT_TRUE(observations1.hasSatellite(sat));
+      ASSERT_TRUE(observations2.hasSatellite(sat));
+  }
 }
 
-
 TEST(ObservationsTest, FindCommonObservationsRemoved)
 {
   Observations observations1;
@@ -148,17 +148,18 @@ TEST(ObservationsTest, FindCommonObservationsRemoved)
   Observations observations2(observations1);
 
   // Remove first observation of observations2
-  observations2.removeObservationByIdx(0);
-
-  Observations common1;
-  Observations common2;
+  int sat_removed = observations2.getObservationByIdx(0).sat;
+  observations2.removeObservationBySat(sat_removed);
 
-  Observations::findCommonObservations(observations1, observations2, common1, common2);
-
-  ASSERT_TRUE(common1 == common2);
-  ASSERT_FALSE(observations1 == common1);
-  ASSERT_TRUE(observations2 == common2);
+  std::set<int> common_sats = Observations::findCommonObservations(observations1, observations2);
 
+  ASSERT_FALSE(common_sats.size() == observations1.size());
+  ASSERT_TRUE(common_sats.size() == observations2.size());
+  for (auto sat : common_sats)
+  {
+      ASSERT_TRUE(observations1.hasSatellite(sat));
+      ASSERT_TRUE(observations2.hasSatellite(sat));
+  }
 }
 
 TEST(ObservationsTest, FindCommonObservationsChangeTime)
@@ -173,34 +174,14 @@ TEST(ObservationsTest, FindCommonObservationsChangeTime)
       obs.time.sec +=10;
   }
 
-  Observations common1;
-  Observations common2;
-
-  Observations::findCommonObservations(observations1, observations2, common1, common2);
-
-  ASSERT_FALSE(common1 == common2);
-  ASSERT_TRUE(observations1 == common1);
-  ASSERT_TRUE(observations2 == common2);
+  std::set<int> common_sats = Observations::findCommonObservations(observations1, observations2);
 
-  for (auto obs = common1.getObservations().begin(); obs != common1.getObservations().end(); ++obs)
+  ASSERT_TRUE(common_sats.size() == observations1.size());
+  ASSERT_TRUE(common_sats.size() == observations2.size());
+  for (auto sat : common_sats)
   {
-      const obsd_t& obs1 = common1.getObservationBySat(obs->sat);
-      const obsd_t& obs2 = common2.getObservationBySat(obs->sat);
-
-      ASSERT_FALSE(obs1.time == obs2.time);
-      ASSERT_TRUE(obs1.eventime == obs2.eventime);
-      ASSERT_TRUE(obs1.timevalid == obs2.timevalid);
-      ASSERT_TRUE(obs1.sat == obs2.sat);
-      ASSERT_TRUE(obs1.rcv == obs2.rcv);
-      ASSERT_TRUE(memcmp(obs1.SNR, obs2.SNR, sizeof(obs1.SNR)) == 0);
-      ASSERT_TRUE(memcmp(obs1.LLI, obs2.LLI, sizeof(obs1.LLI)) == 0);
-      ASSERT_TRUE(memcmp(obs1.code, obs2.code, sizeof(obs1.code)) == 0);
-      ASSERT_TRUE(memcmp(obs1.qualL, obs2.qualL, sizeof(obs1.qualL)) == 0);
-      ASSERT_TRUE(memcmp(obs1.qualP, obs2.qualP, sizeof(obs1.qualP)) == 0);
-      ASSERT_TRUE(obs1.freq == obs2.freq);
-      ASSERT_TRUE(equalArray<double>(obs1.L, obs2.L, sizeof(obs1.L) / sizeof(obs1.L[0]), sizeof(obs2.L) / sizeof(obs2.L[0])));
-      ASSERT_TRUE(equalArray<double>(obs1.P, obs2.P, sizeof(obs1.P) / sizeof(obs1.P[0]), sizeof(obs2.P) / sizeof(obs2.P[0])));
-      ASSERT_TRUE(equalArray<float>(obs1.D, obs2.D, sizeof(obs1.D) / sizeof(obs1.D[0]), sizeof(obs2.D) / sizeof(obs2.D[0])));
+      ASSERT_TRUE(observations1.hasSatellite(sat));
+      ASSERT_TRUE(observations2.hasSatellite(sat));
   }
 }
 
diff --git a/test/gtest_transformations.cpp b/test/gtest_transformations.cpp
index f142ba3..cf5a3b0 100644
--- a/test/gtest_transformations.cpp
+++ b/test/gtest_transformations.cpp
@@ -1,5 +1,6 @@
 #include "gtest/utils_gtest.h"
 #include "gnss_utils/utils/satellite.h"
+#include "gnss_utils/utils/transformations.h"
 
 // Geodetic system parameters
 static double kSemimajorAxis = 6378137;
-- 
GitLab