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

WIP

parent d438dde7
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
...@@ -63,7 +63,7 @@ namespace GNSSUtils ...@@ -63,7 +63,7 @@ namespace GNSSUtils
void computeSatellitesPositions(const Observations& obs, void computeSatellitesPositions(const Observations& obs,
const Navigation& nav, const Navigation& nav,
const prcopt_t& opt, const prcopt_t& opt,
std::map<unsigned char,Eigen::Vector3d>& sats_pos); std::map<int,Eigen::Vector3d>& sats_pos);
} }
#endif #endif
...@@ -40,24 +40,24 @@ namespace GNSSUtils ...@@ -40,24 +40,24 @@ namespace GNSSUtils
bool singleDifferences(const Observations& obs_r, Navigation& nav_r, bool singleDifferences(const Observations& obs_r, Navigation& nav_r,
const Observations& obs_k, const Navigation& nav_k, const Observations& obs_k, const Navigation& nav_k,
Eigen::Vector4d& d, Eigen::Matrix4d& cov_d, 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); const SingleDifferencesParams& sd_opt, const prcopt_t& opt);
bool singleDifferences(const Observations& obs_r, const Navigation& nav_r, const Eigen::Vector3d& x_r, bool singleDifferences(const Observations& obs_r, const Navigation& nav_r, const Eigen::Vector3d& x_r,
const Observations& obs_k, const Navigation& nav_k, const Observations& obs_k, const Navigation& nav_k,
Eigen::Vector4d& d, Eigen::Matrix4d& cov_d, 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); 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, 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<unsigned char,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<unsigned char>& discarded_sats, double& residual, std::set<int>& discarded_sats,
const SingleDifferencesParams& sd_opt, const prcopt_t& opt); const SingleDifferencesParams& sd_opt, const prcopt_t& opt);
void filterCommonSatellites(Observations& common_obs_r, std::map<unsigned char,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<unsigned char,Eigen::Vector3d>& common_sats_pos_k, Observations& common_obs_k, std::map<int,Eigen::Vector3d>& common_sats_pos_k,
std::set<unsigned char>& 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);
} }
......
...@@ -285,7 +285,7 @@ double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Ve ...@@ -285,7 +285,7 @@ double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Ve
void computeSatellitesPositions(const Observations& obs, void computeSatellitesPositions(const Observations& obs,
const Navigation& nav, const Navigation& nav,
const prcopt_t& opt, 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()]; double rs[6*obs.size()],dts[2*obs.size()],var[obs.size()];
int svh[obs.size()]; int svh[obs.size()];
......
...@@ -13,7 +13,7 @@ namespace GNSSUtils ...@@ -13,7 +13,7 @@ namespace GNSSUtils
bool singleDifferences(const Observations& obs_r, Navigation& nav_r, bool singleDifferences(const Observations& obs_r, Navigation& nav_r,
const Observations& obs_k, const Navigation& nav_k, const Observations& obs_k, const Navigation& nav_k,
Eigen::Vector4d& d, Eigen::Matrix4d& cov_d, 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) const SingleDifferencesParams& sd_params, const prcopt_t& opt)
{ {
// COMPUTE SINGLE DIFFERENCES // COMPUTE SINGLE DIFFERENCES
...@@ -27,7 +27,7 @@ bool singleDifferences(const Observations& obs_r, Navigation& nav_r, ...@@ -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, bool singleDifferences(const Observations& obs_r, const Navigation& nav_r, const Eigen::Vector3d& x_r,
const Observations& obs_k, const Navigation& nav_k, const Observations& obs_k, const Navigation& nav_k,
Eigen::Vector4d& d, Eigen::Matrix4d& cov_d, 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) const SingleDifferencesParams& sd_params, const prcopt_t& opt)
{ {
// FIND COMMON SATELLITES OBSERVATIONS // FIND COMMON SATELLITES OBSERVATIONS
...@@ -36,7 +36,7 @@ bool singleDifferences(const Observations& obs_r, const Navigation& nav_r, const ...@@ -36,7 +36,7 @@ bool singleDifferences(const Observations& obs_r, const Navigation& nav_r, const
int n_common_sats = common_obs_r.getObservations().size(); int n_common_sats = common_obs_r.getObservations().size();
// COMPUTE COMMON SATELLITES POSITION // 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_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); 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 ...@@ -54,21 +54,21 @@ bool singleDifferences(const Observations& obs_r, const Navigation& nav_r, const
sd_params, opt); 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, 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<unsigned char,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<unsigned char>& excluded_sats, double& residual, std::set<int>& discarded_sats,
const SingleDifferencesParams& sd_params, const prcopt_t& opt) const SingleDifferencesParams& sd_params, const prcopt_t& opt)
{ {
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 (!excluded_sats.empty()) // 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."); // 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(); // 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);
...@@ -164,7 +164,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r ...@@ -164,7 +164,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
auto obs_k = common_obs_k.getObservationBySat(sat_number); auto obs_k = common_obs_k.getObservationBySat(sat_number);
// excluded satellite // excluded satellite
if (excluded_sats.count(sat_number) != 0) if (discarded_sats.count(sat_number) != 0)
continue; continue;
// Satellite's positions // Satellite's positions
...@@ -231,8 +231,8 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r ...@@ -231,8 +231,8 @@ 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;
// RAIM ====================================== (only at first iteration) // RAIM ====================================== (only at first iteration) RAIM WITH SATELLITES, not ROWS
//TODO: 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) if(j==0 and sd_params.raim_n > 0 and residual > sd_params.raim_min_residual)
{ {
int worst_sat_row = -1; int worst_sat_row = -1;
...@@ -241,7 +241,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r ...@@ -241,7 +241,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
int n_removed_rows = 1; int n_removed_rows = 1;
// remove some satellites // 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 A_raim = A;
auto r_raim = r; auto r_raim = r;
...@@ -297,7 +297,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r ...@@ -297,7 +297,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
} }
// store removed sat // 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 // decrease n_common_sats
n_differences=-n_removed_rows; n_differences=-n_removed_rows;
...@@ -332,7 +332,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r ...@@ -332,7 +332,7 @@ 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 << "SD After RAIM iteration" << std::endl; std::cout << "SD After RAIM iteration" << std::endl;
std::cout << "\tExcluded sats : "; std::cout << "\tExcluded sats : ";
for (auto dsat : excluded_sats) for (auto dsat : discarded_sats)
std::cout << (int)dsat << " "; std::cout << (int)dsat << " ";
std::cout << std::endl; std::cout << std::endl;
std::cout << "\tCommon sats : " << n_common_sats << 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 ...@@ -347,7 +347,7 @@ 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 << "SD After RAIM " << std::endl; std::cout << "SD After RAIM " << std::endl;
std::cout << "\tExcluded sats : "; std::cout << "\tExcluded sats : ";
for (auto dsat : excluded_sats) for (auto dsat : discarded_sats)
std::cout << (int)dsat << " "; std::cout << (int)dsat << " ";
std::cout << std::endl; std::cout << std::endl;
std::cout << "\tCommon sats : " << n_common_sats << 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 ...@@ -362,7 +362,7 @@ 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 << "SD iteration " << j << std::endl; std::cout << "SD iteration " << j << std::endl;
std::cout << "\tExcluded sats : "; std::cout << "\tExcluded sats : ";
for (auto dsat : excluded_sats) for (auto dsat : discarded_sats)
std::cout << (int)dsat << " "; std::cout << (int)dsat << " ";
std::cout << std::endl; std::cout << std::endl;
std::cout << "\tCommon sats : " << n_common_sats << 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 ...@@ -395,9 +395,9 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
return true; return true;
} }
void filterCommonSatellites(Observations& common_obs_r, std::map<unsigned char,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<unsigned char,Eigen::Vector3d>& common_sats_pos_k, Observations& common_obs_k, std::map<int,Eigen::Vector3d>& common_sats_pos_k,
std::set<unsigned char>& 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.size() == common_obs_k.size()); assert(common_obs_r.size() == common_obs_k.size());
......
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