From a3b283f83d27199ead95416bdbd85665b2cb1b44 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Thu, 23 Jan 2020 17:30:43 +0100
Subject: [PATCH] SD working as eurecat, RAIM not tested

---
 include/gnss_utils/gnss_utils.h         |   1 +
 include/gnss_utils/observations.h       |  21 +--
 include/gnss_utils/single_differences.h |   8 +-
 src/gnss_utils.cpp                      |  12 +-
 src/navigation.cpp                      |  18 +++
 src/observations.cpp                    |  74 +++++++++-
 src/single_differences.cpp              | 189 +++++++++++++++---------
 7 files changed, 227 insertions(+), 96 deletions(-)

diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index 4835079..10b7e90 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/gnss_utils.h
@@ -1,6 +1,7 @@
 #ifndef GNSS_UTILS_H
 #define GNSS_UTILS_H
 
+
 #include <vector>
 #include <iostream>
 #include <memory>
diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h
index 9c72e9d..79d460f 100644
--- a/include/gnss_utils/observations.h
+++ b/include/gnss_utils/observations.h
@@ -5,6 +5,7 @@
 #include <map>
 #include <iostream>
 #include <memory>
+#include <cassert>
 
 #include "gnss_utils/utils.h"
 
@@ -64,26 +65,12 @@ class Observations
     private:
         // Private objects
         std::map<unsigned char,int> sat_2_idx_; //< key: corresponding sat number, value: idx in obs_ vector
-        std::map<int,unsigned char> idx_2_sat_; //< key: idx in obs_ vector, value: corresponding sat number
+        std::vector<unsigned char> idx_2_sat_; //< key: idx in obs_ vector, value: corresponding sat number
         std::vector<obsd_t> obs_; //< vector of RTKLIB observations for a given epoch
 
         // Private methods
 };
 
-inline void Observations::removeObservationByIdx(const int& _idx)
-{
-    obs_.erase(obs_.begin() + _idx);
-    sat_2_idx_.erase(idx_2_sat_.at(_idx));
-    idx_2_sat_.erase(_idx);
-}
-
-inline void Observations::removeObservationBySat(const int& _sat)
-{
-    obs_.erase(obs_.begin() + sat_2_idx_.at(_sat));
-    idx_2_sat_.erase(sat_2_idx_.at(_sat));
-    sat_2_idx_.erase(_sat);
-}
-
 inline const std::vector<obsd_t>& Observations::getObservations() const
 {
     return this->obs_;
@@ -96,21 +83,25 @@ inline std::vector<obsd_t>& Observations::getObservations()
 
 inline obsd_t& Observations::getObservationBySat(const unsigned char& sat_number)
 {
+    assert(sat_2_idx_.count(sat_number) != 0);
     return obs_.at(sat_2_idx_.at(sat_number));
 }
 
 inline const obsd_t& Observations::getObservationBySat(const unsigned char& sat_number) const
 {
+    assert(sat_2_idx_.count(sat_number) != 0);
     return obs_.at(sat_2_idx_.at(sat_number));
 }
 
 inline obsd_t& Observations::getObservationByIdx(const int& idx)
 {
+    assert(obs_.size() > idx);
     return obs_.at(idx);
 }
 
 inline const obsd_t& Observations::getObservationByIdx(const int& idx) const
 {
+    assert(obs_.size() > idx);
     return obs_.at(idx);
 }
 
diff --git a/include/gnss_utils/single_differences.h b/include/gnss_utils/single_differences.h
index c62e30b..ac2b68a 100644
--- a/include/gnss_utils/single_differences.h
+++ b/include/gnss_utils/single_differences.h
@@ -8,16 +8,12 @@
 #ifndef INCLUDE_GNSS_UTILS_SINGLE_DIFFERENCES_H_
 #define INCLUDE_GNSS_UTILS_SINGLE_DIFFERENCES_H_
 
-#define GNSS_UTILS_SD_DEBUG 1
+#define GNSS_UTILS_SD_DEBUG 0
 
 #include <set>
 #include "gnss_utils/gnss_utils.h"
 #include "gnss_utils/observations.h"
 #include "gnss_utils/navigation.h"
-extern "C"
-{
-    #include "rtklib.h"
-}
 
 namespace GNSSUtils
 {
@@ -53,7 +49,7 @@ namespace GNSSUtils
                            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 SingleDifferencesParams& sd_opt, const prcopt_t& opt);
+                           const SingleDifferencesParams& 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,
diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp
index ebc946a..93ef9a3 100644
--- a/src/gnss_utils.cpp
+++ b/src/gnss_utils.cpp
@@ -275,8 +275,8 @@ double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Ve
     // receiver-sat vector enu (receiver ecef as origin)
     Eigen::Vector3d receiver_sat_enu;
     ecef2enu(receiver_geo.data(),      //geodetic position {lat,lon} (rad)
-           receiver_ecef.data(),     //vector in ecef coordinate {x,y,z}
-           receiver_sat_enu.data()); // vector in local tangental coordinate {e,n,u}
+             receiver_sat_ecef.data(),     //vector in ecef coordinate {x,y,z}
+             receiver_sat_enu.data()); // vector in local tangental coordinate {e,n,u}
 
     // elevation
     return atan2(receiver_sat_enu(2), sqrt(receiver_sat_enu(0) * receiver_sat_enu(0) + receiver_sat_enu(1) * receiver_sat_enu(1)));
@@ -290,6 +290,11 @@ void computeSatellitesPositions(const Observations& obs,
     double rs[6*obs.size()],dts[2*obs.size()],var[obs.size()];
     int svh[obs.size()];
 
+    //std::cout << "computing sats position from sats: ";
+    //for (auto&& obs_ref : obs.getObservations())
+    //    std::cout << (int)obs_ref.sat << " ";
+    //std::cout << std::endl;
+
     // compute positions
     satposs(obs.getObservations().front().time,
             obs.data(),
@@ -299,13 +304,14 @@ void computeSatellitesPositions(const Observations& obs,
             rs, dts, var, svh);
 
     // store positions
+    //std::cout << "filling sats positions: \n";
     for (int i = 0; i < obs.size(); i++)
     {
         if (svh[i] < 0) // ephemeris unavailable
             sats_pos[obs.getObservationByIdx(i).sat] = Eigen::Vector3d::Zero();
         else
             sats_pos[obs.getObservationByIdx(i).sat] << rs[6*i], rs[6*i+1], rs[6*i+2];
-
+        //std::cout << "\tsat: " << (int)obs.getObservationByIdx(i).sat << ": " << sats_pos[obs.getObservationByIdx(i).sat].transpose() << std::endl;
     }
 }
 }
diff --git a/src/navigation.cpp b/src/navigation.cpp
index f9326b7..fac3874 100644
--- a/src/navigation.cpp
+++ b/src/navigation.cpp
@@ -6,6 +6,22 @@ using namespace GNSSUtils;
 
 Navigation::Navigation()
 {
+
+    nav_.eph =NULL;
+    nav_.geph=NULL;
+    nav_.seph=NULL;
+    nav_.peph=NULL;
+    nav_.pclk=NULL;
+    nav_.alm =NULL;
+    nav_.tec =NULL;
+    nav_.n =nav_.nmax =0;
+    nav_.ng=nav_.ngmax=0;
+    nav_.ns=nav_.nsmax=0;
+    nav_.ne=nav_.nemax=0;
+    nav_.nc=nav_.ncmax=0;
+    nav_.na=nav_.namax=0;
+    nav_.nt=nav_.ntmax=0;
+
 //    allocateEphemeris(0);
 //    allocateGLONASSEphemeris(0);
 //    allocateSBASEphemeris(0);
@@ -191,6 +207,8 @@ bool Navigation::addAlmanac(const alm_t& alm)
 //{
 //    std::cout << "Navigation::deleteAlmanac...\n";
 //  free(nav_.alm);
+//  nav_.na = 0;
+//  nav_.namax = 0;
 //  std::cout << "deleted!\n";
 //}
 
diff --git a/src/observations.cpp b/src/observations.cpp
index 59d1dfb..b6cb50c 100644
--- a/src/observations.cpp
+++ b/src/observations.cpp
@@ -23,10 +23,19 @@ void Observations::clearObservations()
 
 void Observations::addObservation(const obsd_t & obs)
 {
+    assert(sat_2_idx_.size() == idx_2_sat_.size());
+    assert(sat_2_idx_.size() == obs_.size());
+
+
     assert(!hasSatellite(obs.sat) && "adding an observation of a satellite already stored!");
     this->obs_.push_back(obs);
-    idx_2_sat_[obs_.size()-1] = obs.sat;
+    idx_2_sat_.push_back(obs.sat);
     sat_2_idx_[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)
@@ -57,6 +66,54 @@ void Observations::loadFromRinex(const std::string& rnx_file, gtime_t t_start, g
     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());
+}
+
+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());
+}
+
 void Observations::print(obsd_t & _obs)
 {
   std::string msg = "Observation of satellite #" + std::to_string(_obs.sat);
@@ -95,6 +152,15 @@ void Observations::findCommonObservations(const Observations& obs_1, const Obser
     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))
@@ -102,4 +168,10 @@ void Observations::findCommonObservations(const Observations& obs_1, const Obser
             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/single_differences.cpp b/src/single_differences.cpp
index 953840b..71a7032 100644
--- a/src/single_differences.cpp
+++ b/src/single_differences.cpp
@@ -51,25 +51,19 @@ bool singleDifferences(const Observations& obs_r, const Navigation& nav_r, const
                              common_obs_k, nav_k, common_sats_pos_k,
                              d, cov_d,
                              residual, discarded_sats,
-                             sd_params, opt);
+                             sd_params);
 }
 
 bool singleDifferences(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 SingleDifferencesParams& sd_params, const prcopt_t& opt)
+                       const SingleDifferencesParams& 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());
 
-//    if (!discarded_sats.empty())
-//    {
-//        printf("SD: WARNING! excluded_sats list is not empty. It is an output parameter of the RAIM excluded sats. Clearing the list.");
-//        discarded_sats.clear();
-//    }
-
     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);
 
@@ -119,7 +113,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
                 row++;
             }
             // E5 (GAL)
-            if (NFREQ >= 3 and (sys&SYS_GAL) and
+            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)
             {
@@ -144,6 +138,10 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
     #if GNSS_UTILS_SD_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
 
     // FILL SATELLITES POSITIONS AND MEASUREMENTS =======================================================================
@@ -165,18 +163,35 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
 
         // excluded satellite
         if (discarded_sats.count(sat_number) != 0)
+        {
+            #if GNSS_UTILS_SD_DEBUG == 1
+            std::cout << "\tdiscarded sat" << std::endl;
+            #endif
             continue;
+        }
 
         // Satellite's positions
         s_r.col(row) = common_sats_pos_r.at(sat_number);
         s_k.col(row) = common_sats_pos_k.at(sat_number);
 
         if(sd_params.use_carrier_phase)
-            drho_m(row) = (obs_r.L[freq] * nav_r.getNavigation().lam[sat_number][freq]) -
-                          (obs_k.L[freq] * nav_k.getNavigation().lam[sat_number][freq]);
+            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_r.P[freq] - obs_k.P[freq];
+            drho_m(row) = obs_k.P[freq] - obs_r.P[freq];
+
+        #if GNSS_UTILS_SD_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)
         {
@@ -231,6 +246,19 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
         //std::cout << "residual = " << residual << std::endl;
         //std::cout << "SD2: r-A*d = " << (r + A * d).transpose() << std::endl;
 
+#if GNSS_UTILS_SD_DEBUG == 1
+        std::cout << "Displacement vector = %s" << 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("SD: residual      = %13.10f\n", residual);
+        printf("SD: row           = %i\n", r.rows());
+        std::cout << "SD: drho          = " << r.transpose() << std::endl;
+        std::cout << "SD: drho_m        = " << drho_m.transpose() << std::endl;
+        std::cout << "SD: H             = \n" << A << std::endl;
+        printf("SD: 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)
@@ -400,10 +428,16 @@ void filterCommonSatellites(Observations& common_obs_r, std::map<int,Eigen::Vect
                             std::set<int>& discarded_sats, const Eigen::Vector3d& x_r,
                             const SingleDifferencesParams& sd_params, const prcopt_t& opt)
 {
+    assert(&common_obs_r != &common_obs_k);
+    assert(&common_sats_pos_r != &common_sats_pos_k);
     assert(common_obs_r.size() == common_obs_k.size());
     assert(common_obs_r.size() == common_sats_pos_r.size());
     assert(common_obs_r.size() == common_sats_pos_k.size());
 
+    //std::cout << "filterCommonSatellites: initial size: " << common_obs_k.size() << std::endl;
+
+    std::set<int> remove_sats;
+
     for (int obs_i = 0; obs_i < common_obs_r.size(); obs_i++)
     {
         auto&& obs_r = common_obs_r.getObservationByIdx(obs_i);
@@ -411,73 +445,86 @@ void filterCommonSatellites(Observations& common_obs_r, std::map<int,Eigen::Vect
         assert(obs_r.sat == obs_k.sat && "common satellites observations have to be ordered equally");
         const int& sat_number = obs_r.sat;
 
-
-        bool discard = false;
-
         // already discarded sats
         if (discarded_sats.count(sat_number) != 0)
-            discard = true;
-        else
         {
-            // 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))
-            {
-                discard = true;
-            }
-            else if (!sd_params.use_carrier_phase and (std::abs(obs_r.P[0]) < 1e-12 or std::abs(obs_k.L[0]) < 1e-12))
-            {
-                discard = true;
-            }
-            else
-            {
-                // bad satellite position (satellite is not included in the discarded list)
-                if (common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero()) or
-                    common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero()))
-                {
-                    discard = true;
-                }
-                else
-                {
-                    // constellation
-                    int sys=satsys(obs_r.sat,NULL);
-                    if (!(sys & opt.navsys))
-                    {
-                        discarded_sats.insert(sat_number);
-                        discard = true;
-                    }
-                    else
-                    {
-                        // 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)
-                        {
-                            discarded_sats.insert(sat_number);
-                            discard = true;
-                        }
-                        else
-                        {
-                            // snr TODO: multifrequency (2nd param and 3rd idx)
-                            if (testsnr(0, 0, elevation, obs_r.SNR[0] * 0.25, &opt.snrmask) == 1)
-                            {
-                                discarded_sats.insert(sat_number);
-                                discard = true;
-                            }
-                        }
-                    }
-                }
-            }
+            remove_sats.insert(sat_number);
+            continue;
+        }
+
+        // wrong data (satellite is not included in the discarded list)
+        if (sd_params.use_carrier_phase and (std::abs(obs_r.L[0]) < 1e-12 or std::abs(obs_k.L[0]) < 1e-12))
+        {
+            // std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_r.L[0] << " " << obs_k.L[0] << std::endl;
+            remove_sats.insert(sat_number);
+            continue;
+        }
+        if (!sd_params.use_carrier_phase and (std::abs(obs_r.P[0]) < 1e-12 or std::abs(obs_k.L[0]) < 1e-12))
+        {
+            // std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_r.P[0] << " " << obs_k.P[0] << std::endl;
+            remove_sats.insert(sat_number);
+            continue;
+        }
+
+        // bad satellite position (satellite is not included in the discarded list)
+        if (common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3) or
+            common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3))
+        {
+            // std::cout << "Discarding sat " << sat_number << ": bad satellite position: \n\t" << common_sats_pos_r[sat_number].transpose() << "\n\t" << common_sats_pos_k[sat_number].transpose() << std::endl;
+            remove_sats.insert(sat_number);
+            continue;
+        }
+
+        // constellation
+        int sys=satsys(obs_r.sat,NULL);
+        if (!(sys & opt.navsys))
+        {
+            // std::cout << "Discarding sat " << sat_number << ": not selected constellation " << sys << " - mask: " << opt.navsys << std::endl;
+            discarded_sats.insert(sat_number);
+            remove_sats.insert(sat_number);
+            continue;
+        }
+
+        // check both elevations
+        double elevation = std::min(computeSatElevation(x_r, common_sats_pos_r[sat_number]),
+                                    computeSatElevation(x_r, common_sats_pos_k[sat_number]));
+        if (elevation < opt.elmin)
+        {
+            // std::cout << "Discarding sat " << sat_number << ": low elevation " << elevation << " - min: " << opt.elmin << std::endl;
+            discarded_sats.insert(sat_number);
+            remove_sats.insert(sat_number);
+            continue;
         }
 
-        // remove from vectors
-        if (discard)
+        // snr TODO: multifrequency (2nd param and 3rd idx)
+        if (testsnr(0, 0, elevation, obs_r.SNR[0] * 0.25, &opt.snrmask) == 1)
         {
-            common_obs_r.removeObservationByIdx(obs_i);
-            common_obs_k.removeObservationByIdx(obs_i);
-            common_sats_pos_r.erase(sat_number);
-            common_sats_pos_k.erase(sat_number);
+            // 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;
 }
 
 }
-- 
GitLab