Skip to content
Snippets Groups Projects
Commit a3b283f8 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

SD working as eurecat, RAIM not tested

parent e7fb40bb
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
#ifndef GNSS_UTILS_H #ifndef GNSS_UTILS_H
#define GNSS_UTILS_H #define GNSS_UTILS_H
#include <vector> #include <vector>
#include <iostream> #include <iostream>
#include <memory> #include <memory>
......
...@@ -5,6 +5,7 @@ ...@@ -5,6 +5,7 @@
#include <map> #include <map>
#include <iostream> #include <iostream>
#include <memory> #include <memory>
#include <cassert>
#include "gnss_utils/utils.h" #include "gnss_utils/utils.h"
...@@ -64,26 +65,12 @@ class Observations ...@@ -64,26 +65,12 @@ class Observations
private: private:
// Private objects // Private objects
std::map<unsigned char,int> sat_2_idx_; //< key: corresponding sat number, value: idx in obs_ vector 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 std::vector<obsd_t> obs_; //< vector of RTKLIB observations for a given epoch
// Private methods // 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 inline const std::vector<obsd_t>& Observations::getObservations() const
{ {
return this->obs_; return this->obs_;
...@@ -96,21 +83,25 @@ inline std::vector<obsd_t>& Observations::getObservations() ...@@ -96,21 +83,25 @@ inline std::vector<obsd_t>& Observations::getObservations()
inline obsd_t& Observations::getObservationBySat(const unsigned char& sat_number) 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)); return obs_.at(sat_2_idx_.at(sat_number));
} }
inline const obsd_t& Observations::getObservationBySat(const unsigned char& sat_number) const 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)); return obs_.at(sat_2_idx_.at(sat_number));
} }
inline obsd_t& Observations::getObservationByIdx(const int& idx) inline obsd_t& Observations::getObservationByIdx(const int& idx)
{ {
assert(obs_.size() > idx);
return obs_.at(idx); return obs_.at(idx);
} }
inline const obsd_t& Observations::getObservationByIdx(const int& idx) const inline const obsd_t& Observations::getObservationByIdx(const int& idx) const
{ {
assert(obs_.size() > idx);
return obs_.at(idx); return obs_.at(idx);
} }
......
...@@ -8,16 +8,12 @@ ...@@ -8,16 +8,12 @@
#ifndef INCLUDE_GNSS_UTILS_SINGLE_DIFFERENCES_H_ #ifndef INCLUDE_GNSS_UTILS_SINGLE_DIFFERENCES_H_
#define 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 <set>
#include "gnss_utils/gnss_utils.h" #include "gnss_utils/gnss_utils.h"
#include "gnss_utils/observations.h" #include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h" #include "gnss_utils/navigation.h"
extern "C"
{
#include "rtklib.h"
}
namespace GNSSUtils namespace GNSSUtils
{ {
...@@ -53,7 +49,7 @@ 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, 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, Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
double& residual, std::set<int>& discarded_sats, 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, 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, Observations& common_obs_k, std::map<int,Eigen::Vector3d>& common_sats_pos_k,
......
...@@ -275,8 +275,8 @@ double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Ve ...@@ -275,8 +275,8 @@ double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Ve
// receiver-sat vector enu (receiver ecef as origin) // receiver-sat vector enu (receiver ecef as origin)
Eigen::Vector3d receiver_sat_enu; Eigen::Vector3d receiver_sat_enu;
ecef2enu(receiver_geo.data(), //geodetic position {lat,lon} (rad) ecef2enu(receiver_geo.data(), //geodetic position {lat,lon} (rad)
receiver_ecef.data(), //vector in ecef coordinate {x,y,z} receiver_sat_ecef.data(), //vector in ecef coordinate {x,y,z}
receiver_sat_enu.data()); // vector in local tangental coordinate {e,n,u} receiver_sat_enu.data()); // vector in local tangental coordinate {e,n,u}
// elevation // elevation
return atan2(receiver_sat_enu(2), sqrt(receiver_sat_enu(0) * receiver_sat_enu(0) + receiver_sat_enu(1) * receiver_sat_enu(1))); 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, ...@@ -290,6 +290,11 @@ void computeSatellitesPositions(const Observations& obs,
double rs[6*obs.size()],dts[2*obs.size()],var[obs.size()]; double rs[6*obs.size()],dts[2*obs.size()],var[obs.size()];
int svh[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 // compute positions
satposs(obs.getObservations().front().time, satposs(obs.getObservations().front().time,
obs.data(), obs.data(),
...@@ -299,13 +304,14 @@ void computeSatellitesPositions(const Observations& obs, ...@@ -299,13 +304,14 @@ void computeSatellitesPositions(const Observations& obs,
rs, dts, var, svh); rs, dts, var, svh);
// store positions // store positions
//std::cout << "filling sats positions: \n";
for (int i = 0; i < obs.size(); i++) for (int i = 0; i < obs.size(); i++)
{ {
if (svh[i] < 0) // ephemeris unavailable if (svh[i] < 0) // ephemeris unavailable
sats_pos[obs.getObservationByIdx(i).sat] = Eigen::Vector3d::Zero(); sats_pos[obs.getObservationByIdx(i).sat] = Eigen::Vector3d::Zero();
else else
sats_pos[obs.getObservationByIdx(i).sat] << rs[6*i], rs[6*i+1], rs[6*i+2]; 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;
} }
} }
} }
...@@ -6,6 +6,22 @@ using namespace GNSSUtils; ...@@ -6,6 +6,22 @@ using namespace GNSSUtils;
Navigation::Navigation() 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); // allocateEphemeris(0);
// allocateGLONASSEphemeris(0); // allocateGLONASSEphemeris(0);
// allocateSBASEphemeris(0); // allocateSBASEphemeris(0);
...@@ -191,6 +207,8 @@ bool Navigation::addAlmanac(const alm_t& alm) ...@@ -191,6 +207,8 @@ bool Navigation::addAlmanac(const alm_t& alm)
//{ //{
// std::cout << "Navigation::deleteAlmanac...\n"; // std::cout << "Navigation::deleteAlmanac...\n";
// free(nav_.alm); // free(nav_.alm);
// nav_.na = 0;
// nav_.namax = 0;
// std::cout << "deleted!\n"; // std::cout << "deleted!\n";
//} //}
......
...@@ -23,10 +23,19 @@ void Observations::clearObservations() ...@@ -23,10 +23,19 @@ void Observations::clearObservations()
void Observations::addObservation(const obsd_t & obs) void Observations::addObservation(const obsd_t & obs)
{ {
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
assert(!hasSatellite(obs.sat) && "adding an observation of a satellite already stored!"); assert(!hasSatellite(obs.sat) && "adding an observation of a satellite already stored!");
this->obs_.push_back(obs); 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; 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) 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 ...@@ -57,6 +66,54 @@ void Observations::loadFromRinex(const std::string& rnx_file, gtime_t t_start, g
free(obs.data); free(obs.data);
} }
void Observations::removeObservationByIdx(const int& _idx)
{
//std::cout << "removing observation of idx " << _idx << std::endl;
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
assert(obs_.size() > _idx);
assert(idx_2_sat_.size() > _idx);
assert(sat_2_idx_.count(idx_2_sat_.at(_idx)) != 0);
// remove obs from sat_2_idx map
sat_2_idx_.erase(idx_2_sat_.at(_idx));
// decrease the idx of the observations after the removed one
for (auto&& sat_idx : sat_2_idx_)
if (sat_idx.second > _idx)
sat_idx.second--;
// remove obs from obs and idx_2_sat vectors
obs_.erase(obs_.begin() + _idx);
idx_2_sat_.erase(idx_2_sat_.begin() + _idx);
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
}
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) void Observations::print(obsd_t & _obs)
{ {
std::string msg = "Observation of satellite #" + std::to_string(_obs.sat); std::string msg = "Observation of satellite #" + std::to_string(_obs.sat);
...@@ -95,6 +152,15 @@ void Observations::findCommonObservations(const Observations& obs_1, const Obser ...@@ -95,6 +152,15 @@ void Observations::findCommonObservations(const Observations& obs_1, const Obser
common_obs_1.clearObservations(); common_obs_1.clearObservations();
common_obs_2.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 // iterate 1
for (auto&& obs_1_ref : obs_1.getObservations()) for (auto&& obs_1_ref : obs_1.getObservations())
if (obs_2.hasSatellite(obs_1_ref.sat)) if (obs_2.hasSatellite(obs_1_ref.sat))
...@@ -102,4 +168,10 @@ void Observations::findCommonObservations(const Observations& obs_1, const Obser ...@@ -102,4 +168,10 @@ void Observations::findCommonObservations(const Observations& obs_1, const Obser
common_obs_1.addObservation(obs_1_ref); common_obs_1.addObservation(obs_1_ref);
common_obs_2.addObservation(obs_2.getObservationBySat(obs_1_ref.sat)); 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;
} }
...@@ -51,25 +51,19 @@ bool singleDifferences(const Observations& obs_r, const Navigation& nav_r, const ...@@ -51,25 +51,19 @@ bool singleDifferences(const Observations& obs_r, const Navigation& nav_r, const
common_obs_k, nav_k, common_sats_pos_k, common_obs_k, nav_k, common_sats_pos_k,
d, cov_d, d, cov_d,
residual, discarded_sats, 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, 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, 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, Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
double& residual, std::set<int>& discarded_sats, 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_obs_k.size());
assert(common_obs_r.size() == common_sats_pos_r.size()); assert(common_obs_r.size() == common_sats_pos_r.size());
assert(common_obs_k.size() == common_sats_pos_k.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 tr(common_obs_r.getObservations().front().time.time + common_obs_r.getObservations().front().time.sec);
double tk(common_obs_k.getObservations().front().time.time + common_obs_k.getObservations().front().time.sec); double 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 ...@@ -119,7 +113,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
row++; row++;
} }
// E5 (GAL) // 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_r.L[2]) > 1e-12 and
std::abs(obs_k.L[2]) > 1e-12) std::abs(obs_k.L[2]) > 1e-12)
{ {
...@@ -144,6 +138,10 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r ...@@ -144,6 +138,10 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
#if GNSS_UTILS_SD_DEBUG == 1 #if GNSS_UTILS_SD_DEBUG == 1
std::cout << "Receiver at t_r: " << x_r.transpose() << std::endl; std::cout << "Receiver at t_r: " << x_r.transpose() << std::endl;
std::cout << "d initial guess: " << d_0.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 #endif
// FILL SATELLITES POSITIONS AND MEASUREMENTS ======================================================================= // FILL SATELLITES POSITIONS AND MEASUREMENTS =======================================================================
...@@ -165,18 +163,35 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r ...@@ -165,18 +163,35 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
// excluded satellite // excluded satellite
if (discarded_sats.count(sat_number) != 0) if (discarded_sats.count(sat_number) != 0)
{
#if GNSS_UTILS_SD_DEBUG == 1
std::cout << "\tdiscarded sat" << std::endl;
#endif
continue; continue;
}
// Satellite's positions // Satellite's positions
s_r.col(row) = common_sats_pos_r.at(sat_number); s_r.col(row) = common_sats_pos_r.at(sat_number);
s_k.col(row) = common_sats_pos_k.at(sat_number); s_k.col(row) = common_sats_pos_k.at(sat_number);
if(sd_params.use_carrier_phase) if(sd_params.use_carrier_phase)
drho_m(row) = (obs_r.L[freq] * nav_r.getNavigation().lam[sat_number][freq]) - drho_m(row) = (obs_k.L[freq] * nav_k.getNavigation().lam[sat_number-1][freq]) -
(obs_k.L[freq] * nav_k.getNavigation().lam[sat_number][freq]); (obs_r.L[freq] * nav_r.getNavigation().lam[sat_number-1][freq]);
else 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) if(!sd_params.relinearize_jacobian)
{ {
...@@ -231,6 +246,19 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r ...@@ -231,6 +246,19 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
//std::cout << "residual = " << residual << std::endl; //std::cout << "residual = " << residual << std::endl;
//std::cout << "SD2: r-A*d = " << (r + A * d).transpose() << 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 // RAIM ====================================== (only at first iteration) RAIM WITH SATELLITES, not ROWS
int input_discarded_sats = discarded_sats.size(); int input_discarded_sats = discarded_sats.size();
if(j==0 and sd_params.raim_n > 0 and residual > sd_params.raim_min_residual) 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 ...@@ -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, std::set<int>& discarded_sats, const Eigen::Vector3d& x_r,
const SingleDifferencesParams& sd_params, const prcopt_t& opt) 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_obs_k.size());
assert(common_obs_r.size() == common_sats_pos_r.size()); assert(common_obs_r.size() == common_sats_pos_r.size());
assert(common_obs_r.size() == common_sats_pos_k.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++) for (int obs_i = 0; obs_i < common_obs_r.size(); obs_i++)
{ {
auto&& obs_r = common_obs_r.getObservationByIdx(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 ...@@ -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"); assert(obs_r.sat == obs_k.sat && "common satellites observations have to be ordered equally");
const int& sat_number = obs_r.sat; const int& sat_number = obs_r.sat;
bool discard = false;
// already discarded sats // already discarded sats
if (discarded_sats.count(sat_number) != 0) if (discarded_sats.count(sat_number) != 0)
discard = true;
else
{ {
// wrong data (satellite is not included in the discarded list) remove_sats.insert(sat_number);
if (sd_params.use_carrier_phase and (std::abs(obs_r.L[0]) < 1e-12 or std::abs(obs_k.L[0]) < 1e-12)) continue;
{ }
discard = true;
} // wrong data (satellite is not included in the discarded list)
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)) 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; // 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);
else continue;
{ }
// bad satellite position (satellite is not included in the discarded list) if (!sd_params.use_carrier_phase and (std::abs(obs_r.P[0]) < 1e-12 or std::abs(obs_k.L[0]) < 1e-12))
if (common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero()) or {
common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero())) // 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);
discard = true; continue;
} }
else
{ // bad satellite position (satellite is not included in the discarded list)
// constellation if (common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3) or
int sys=satsys(obs_r.sat,NULL); common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3))
if (!(sys & opt.navsys)) {
{ // 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;
discarded_sats.insert(sat_number); remove_sats.insert(sat_number);
discard = true; continue;
} }
else
{ // constellation
// check both elevations int sys=satsys(obs_r.sat,NULL);
double elevation = std::min(computeSatElevation(x_r, common_sats_pos_r[sat_number]), if (!(sys & opt.navsys))
computeSatElevation(x_r, common_sats_pos_k[sat_number])); {
if (elevation < opt.elmin) // std::cout << "Discarding sat " << sat_number << ": not selected constellation " << sys << " - mask: " << opt.navsys << std::endl;
{ discarded_sats.insert(sat_number);
discarded_sats.insert(sat_number); remove_sats.insert(sat_number);
discard = true; continue;
} }
else
{ // check both elevations
// snr TODO: multifrequency (2nd param and 3rd idx) double elevation = std::min(computeSatElevation(x_r, common_sats_pos_r[sat_number]),
if (testsnr(0, 0, elevation, obs_r.SNR[0] * 0.25, &opt.snrmask) == 1) computeSatElevation(x_r, common_sats_pos_k[sat_number]));
{ if (elevation < opt.elmin)
discarded_sats.insert(sat_number); {
discard = true; // 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 // snr TODO: multifrequency (2nd param and 3rd idx)
if (discard) if (testsnr(0, 0, elevation, obs_r.SNR[0] * 0.25, &opt.snrmask) == 1)
{ {
common_obs_r.removeObservationByIdx(obs_i); // std::cout << "Discarding sat " << sat_number << ": snr test " << std::endl;
common_obs_k.removeObservationByIdx(obs_i); discarded_sats.insert(sat_number);
common_sats_pos_r.erase(sat_number); remove_sats.insert(sat_number);
common_sats_pos_k.erase(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;
} }
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment