diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h index cbbaa5ee3dfbbad5fb6ba6a15cf9c69dbaf9d221..48350790c77107e7272e36a3908d4daa5d4e2870 100644 --- a/include/gnss_utils/gnss_utils.h +++ b/include/gnss_utils/gnss_utils.h @@ -63,7 +63,7 @@ namespace GNSSUtils void computeSatellitesPositions(const Observations& obs, const Navigation& nav, const prcopt_t& opt, - std::map<unsigned char,Eigen::Vector3d>& sats_pos); + std::map<int,Eigen::Vector3d>& sats_pos); } #endif diff --git a/include/gnss_utils/single_differences.h b/include/gnss_utils/single_differences.h index 3c1ee0851477856c01879781644d47da08cbef4b..c62e30beb0861fadcb7dd964ca5b8f367cad8abb 100644 --- a/include/gnss_utils/single_differences.h +++ b/include/gnss_utils/single_differences.h @@ -40,24 +40,24 @@ namespace GNSSUtils bool singleDifferences(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<unsigned char>& discarded_sats, + double& residual, std::set<int>& discarded_sats, const SingleDifferencesParams& sd_opt, const prcopt_t& opt); bool singleDifferences(const Observations& obs_r, const Navigation& nav_r, const Eigen::Vector3d& x_r, const Observations& obs_k, const Navigation& nav_k, Eigen::Vector4d& d, Eigen::Matrix4d& cov_d, - double& residual, std::set<unsigned char>& discarded_sats, + double& residual, std::set<int>& discarded_sats, const SingleDifferencesParams& sd_opt, const prcopt_t& opt); - bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r, const std::map<unsigned char,Eigen::Vector3d>& common_sats_pos_r, const Eigen::Vector3d& x_r, - const Observations& common_obs_k, const Navigation& nav_k, const std::map<unsigned char,Eigen::Vector3d>& common_sats_pos_k, + 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<unsigned char>& discarded_sats, + double& residual, std::set<int>& discarded_sats, const SingleDifferencesParams& sd_opt, const prcopt_t& opt); - void filterCommonSatellites(Observations& common_obs_r, std::map<unsigned char,Eigen::Vector3d>& common_sats_pos_r, - Observations& common_obs_k, std::map<unsigned char,Eigen::Vector3d>& common_sats_pos_k, - std::set<unsigned char>& discarded_sats, const Eigen::Vector3d& x_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, + std::set<int>& discarded_sats, const Eigen::Vector3d& x_r, const SingleDifferencesParams& sd_params, const prcopt_t& opt); } diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp index ff9621aa036998d61c1003aaaf5626376a7fa7b2..ebc946a67f5d339edd9a8ff41c7c7d0618e36e98 100644 --- a/src/gnss_utils.cpp +++ b/src/gnss_utils.cpp @@ -285,7 +285,7 @@ double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Ve void computeSatellitesPositions(const Observations& obs, const Navigation& nav, const prcopt_t& opt, - std::map<unsigned char,Eigen::Vector3d>& sats_pos) + std::map<int,Eigen::Vector3d>& sats_pos) { double rs[6*obs.size()],dts[2*obs.size()],var[obs.size()]; int svh[obs.size()]; diff --git a/src/single_differences.cpp b/src/single_differences.cpp index dfb97f36b3b571dab24f16c15147817abc806448..953840bd009d47f0f887dd032b86788ce7548068 100644 --- a/src/single_differences.cpp +++ b/src/single_differences.cpp @@ -13,7 +13,7 @@ namespace GNSSUtils bool singleDifferences(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<unsigned char>& discarded_sats, + double& residual, std::set<int>& discarded_sats, const SingleDifferencesParams& sd_params, const prcopt_t& opt) { // COMPUTE SINGLE DIFFERENCES @@ -27,7 +27,7 @@ bool singleDifferences(const Observations& obs_r, Navigation& nav_r, bool singleDifferences(const Observations& obs_r, const Navigation& nav_r, const Eigen::Vector3d& x_r, const Observations& obs_k, const Navigation& nav_k, Eigen::Vector4d& d, Eigen::Matrix4d& cov_d, - double& residual, std::set<unsigned char>& discarded_sats, + double& residual, std::set<int>& discarded_sats, const SingleDifferencesParams& sd_params, const prcopt_t& opt) { // FIND COMMON SATELLITES OBSERVATIONS @@ -36,7 +36,7 @@ bool singleDifferences(const Observations& obs_r, const Navigation& nav_r, const int n_common_sats = common_obs_r.getObservations().size(); // COMPUTE COMMON SATELLITES POSITION - std::map<unsigned char, Eigen::Vector3d> common_sats_pos_r, common_sats_pos_k; + std::map<int, Eigen::Vector3d> common_sats_pos_r, common_sats_pos_k; computeSatellitesPositions(common_obs_r, nav_r, opt, common_sats_pos_r); computeSatellitesPositions(common_obs_k, sd_params.use_old_nav ? nav_r : nav_k, opt, common_sats_pos_k); @@ -54,21 +54,21 @@ bool singleDifferences(const Observations& obs_r, const Navigation& nav_r, const sd_params, opt); } -bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r, const std::map<unsigned char,Eigen::Vector3d>& common_sats_pos_r, const Eigen::Vector3d& x_r, - const Observations& common_obs_k, const Navigation& nav_k, const std::map<unsigned char,Eigen::Vector3d>& common_sats_pos_k, +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<unsigned char>& excluded_sats, + double& residual, std::set<int>& discarded_sats, const SingleDifferencesParams& sd_params, const prcopt_t& opt) { 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 (!excluded_sats.empty()) - { - printf("SD: WARNING! excluded_sats list is not empty. It is an output parameter of the RAIM excluded sats. Clearing the list."); - excluded_sats.clear(); - } +// 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); @@ -164,7 +164,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r auto obs_k = common_obs_k.getObservationBySat(sat_number); // excluded satellite - if (excluded_sats.count(sat_number) != 0) + if (discarded_sats.count(sat_number) != 0) continue; // Satellite's positions @@ -231,8 +231,8 @@ 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; - // RAIM ====================================== (only at first iteration) - //TODO: RAIM WITH SATELLITES, not ROWS + // 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) { int worst_sat_row = -1; @@ -241,7 +241,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r int n_removed_rows = 1; // remove some satellites - while(excluded_sats.size() < sd_params.raim_n) + while(discarded_sats.size() - input_discarded_sats < sd_params.raim_n) { auto A_raim = A; auto r_raim = r; @@ -297,7 +297,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r } // store removed sat - excluded_sats.insert(row_2_sat_freq[worst_sat_row].first); + discarded_sats.insert(row_2_sat_freq[worst_sat_row].first); // decrease n_common_sats n_differences=-n_removed_rows; @@ -332,7 +332,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r #if GNSS_UTILS_SD_DEBUG == 1 std::cout << "SD After RAIM iteration" << std::endl; std::cout << "\tExcluded sats : "; - for (auto dsat : excluded_sats) + for (auto dsat : discarded_sats) std::cout << (int)dsat << " "; std::cout << std::endl; std::cout << "\tCommon sats : " << n_common_sats << std::endl; @@ -347,7 +347,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r #if GNSS_UTILS_SD_DEBUG == 1 std::cout << "SD After RAIM " << std::endl; std::cout << "\tExcluded sats : "; - for (auto dsat : excluded_sats) + for (auto dsat : discarded_sats) std::cout << (int)dsat << " "; std::cout << std::endl; std::cout << "\tCommon sats : " << n_common_sats << std::endl; @@ -362,7 +362,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r #if GNSS_UTILS_SD_DEBUG == 1 std::cout << "SD iteration " << j << std::endl; std::cout << "\tExcluded sats : "; - for (auto dsat : excluded_sats) + for (auto dsat : discarded_sats) std::cout << (int)dsat << " "; std::cout << std::endl; std::cout << "\tCommon sats : " << n_common_sats << std::endl; @@ -395,9 +395,9 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r return true; } -void filterCommonSatellites(Observations& common_obs_r, std::map<unsigned char,Eigen::Vector3d>& common_sats_pos_r, - Observations& common_obs_k, std::map<unsigned char,Eigen::Vector3d>& common_sats_pos_k, - std::set<unsigned char>& discarded_sats, const Eigen::Vector3d& x_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, + std::set<int>& discarded_sats, const Eigen::Vector3d& x_r, const SingleDifferencesParams& sd_params, const prcopt_t& opt) { assert(common_obs_r.size() == common_obs_k.size());