Resolve "TDCP batch implementation"

Closes #15 (closed)

src/observations.cpp

@@ -304,18 +304,19 @@ std::set<int> Observations::filterByConstellations(const int& navsys)
     return remove_sats;
 }
 
-std::set<int> Observations::filterByElevationSnr(const std::map<int,Eigen::Vector2d>& azels,
+std::set<int> Observations::filterByElevationSnr(const Azels& azels,
                                                  const snrmask_t& snrmask,
                                                  const double& elmin,
                                                  const bool &multi_freq)
 {
     std::set<int> remove_sats;
 
-    for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
+    for (auto&& sat_idx_pair : sat_2_idx_)
     {
-      auto&& obs_sat = getObservationByIdx(obs_i);
-      const int& sat_number = obs_sat.sat;
+      const int& sat_number = sat_idx_pair.first;
+      assert(azels.count(sat_number) && "azel missing for a satellite of this observation");
       const double& elevation(azels.at(sat_number)(1));
+      auto&& obs_sat = getObservationByIdx(sat_idx_pair.second);
 
       // check elevation
       if (elevation < elmin)
@@ -354,27 +355,6 @@ std::set<int> Observations::filterByElevationSnr(const std::map<int,Eigen::Vecto
     return remove_sats;
 }
 
-std::set<int> Observations::filterByElevationSnr(const Eigen::Vector3d& x_r,
-                                                 const Satellites& sats,
-                                                 const snrmask_t& snrmask,
-                                                 const double& elmin,
-                                                 const bool &multi_freq)
-{
-    std::set<int> remove_sats;
-    std::map<int,Eigen::Vector2d> azels;
-
-    for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
-    {
-      auto&& obs_sat = getObservationByIdx(obs_i);
-      const int& sat_number = obs_sat.sat;
-
-      double elevation = computeSatElevation(x_r, sats.at(obs_sat.sat).pos);
-      azels.emplace(obs_sat.sat,Eigen::Vector2d(0.0,elevation));
-    }
-
-    return filterByElevationSnr(azels, snrmask, elmin, multi_freq);
-}
-
 std::set<int> Observations::filter(const Satellites&        sats,
                                    const std::set<int>&     discarded_sats,
                                    const Eigen::Vector3d&   x_r,
@@ -403,51 +383,19 @@ std::set<int> Observations::filter(const Satellites&        sats,
                                    const double&            elmin,
                                    const bool&              multi_freq)
 {
-    //std::cout << "filter: initial size: " << obs_.size() << std::endl;
-    // Ephemeris
-    std::set<int> remove_sats = filterByEphemeris(sats);
-
-    // Discarded sats
-    std::set<int> remove_sats_discarded = filterBySatellites(discarded_sats);
-    remove_sats.insert(remove_sats_discarded.begin(), remove_sats_discarded.end());
+    assert(!x_r.isApprox(Eigen::Vector3d::Zero(), 1e-3));
 
-    // Code
-    if (check_code)
-    {
-        std::set<int> remove_sats_code = filterByCode();
-        remove_sats.insert(remove_sats_code.begin(), remove_sats_code.end());
-    }
+    Azels azels = computeAzels(sats, x_r);
 
-    // Carrier phase
-    if (check_carrier_phase)
-    {
-        std::set<int> remove_sats_carrier = filterByCarrierPhase();
-        remove_sats.insert(remove_sats_carrier.begin(), remove_sats_carrier.end());
-    }
-
-    // satellite flag and ephemeris variance
-    for (auto sat_pair : sats)
-        if (remove_sats.count(sat_pair.first) == 0)
-            if (satexclude(sat_pair.first,sat_pair.second.var, sat_pair.second.svh, NULL))
-            {
-                //std::cout << "Discarding sat " << sat_pair.first << ": unhealthy or huge variance svh = " << sat_pair.second.svh << std::endl;
-                removeObservationBySat(sat_pair.first);
-                remove_sats.insert(sat_pair.first);
-            }
-
-    // Constellations
-    std::set<int> remove_sats_constellations = filterByConstellations(navsys);
-    remove_sats.insert(remove_sats_constellations.begin(), remove_sats_constellations.end());
-
-    // Elevation and SNR
-    if (!x_r.isApprox(Eigen::Vector3d::Zero(), 1e-3))
-    {
-        std::set<int> remove_sats_elevation = filterByElevationSnr(x_r, sats, snrmask, elmin, multi_freq);
-        remove_sats.insert(remove_sats_elevation.begin(), remove_sats_elevation.end());
-    }
-
-    return remove_sats;
-    // std::cout << "final size: " << obs_.size() << std::endl;
+    return filter(sats,
+                  discarded_sats,
+                  azels,
+                  check_code,
+                  check_carrier_phase,
+                  navsys,
+                  snrmask,
+                  elmin,
+                  multi_freq);
 }
 
 std::set<int> Observations::filter(const Satellites&    sats,