diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index 48350790c77107e7272e36a3908d4daa5d4e2870..10b7e909fc44d18f95b0cc69006c816e832e571a 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 9c72e9d00dbf91e0a453461710fe70f123a36b2d..79d460f93ca6e136963a9c5ae1d98c258ba00648 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 c62e30beb0861fadcb7dd964ca5b8f367cad8abb..ac2b68abc67571bd8352510333e25a3cde9b5fa0 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 ebc946a67f5d339edd9a8ff41c7c7d0618e36e98..93ef9a3eff7f7c135fa9ba56972fff565186e309 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 f9326b7fbddd9a36dcb92e841e2a2be79e27d27c..fac38742f09fe5dba1bc6aefa7562f665b64d70d 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 59d1dfb0020c5f9ad42aa0a7599f2ee183d8d16b..b6cb50cd2ecf6560eacb662ee0e9489f0c259032 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 953840bd009d47f0f887dd032b86788ce7548068..71a7032c072d0a6be2f70fca7d6f96a3e1096c06 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;
}
}