diff --git a/include/gnss/processor/processor_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h index 31c834b5dc71146d4aa995fb003fd9eddd15a136..b0eb4b5f034fa9ac9b84fec4a805005610e6bc59 100644 --- a/include/gnss/processor/processor_tracker_gnss.h +++ b/include/gnss/processor/processor_tracker_gnss.h @@ -80,9 +80,9 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature } // COMPUTE FIX OPTIONS (RAIM) - fix_opt.elmin = 0; - fix_opt.maxgdop = 30; fix_opt.raim = gnss_opt.raim; + fix_opt.sateph = 6; //EPHOPT_SBAS2; + fix_opt.ionoopt = 9;//IONOPT_SBAS2; // same constellations fix_opt.GPS = gnss_opt.GPS; fix_opt.SBS = gnss_opt.SBS; diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp index 2cb5a773cd2e3c61972376e561f46f7933c72387..a8e7724762670b3a4c9884b8b0dcf84d0e7dfcbe 100644 --- a/src/processor/processor_tracker_gnss.cpp +++ b/src/processor/processor_tracker_gnss.cpp @@ -30,6 +30,9 @@ void ProcessorTrackerGnss::preProcess() #ifdef _WOLF_DEBUG int n_initial = inc_snapshot->getObservations()->size(); + std::string initial_str; + for (auto obs : inc_snapshot->getObservations()->getObservations()) + initial_str += std::to_string(obs.sat) + " "; #endif // compute satellites positions @@ -84,16 +87,16 @@ void ProcessorTrackerGnss::preProcess() WOLF_DEBUG("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.pos.transpose(), " - Fix solution (GEO): ", fix_incoming_.lat_lon.transpose()); - //WOLF_DEBUG("preprocess: RTKLIB excluded observations: ", fix_incoming_.discarded_sats.size()); // filter observations (available ephemeris, constellations and elevation&SNR) + #ifdef _WOLF_DEBUG + auto discarded_gnssutils = + #endif inc_snapshot->filterObservations(fix_incoming_.discarded_sats, // discarded sats fix_incoming_.sat_azel, false, // check code false, // check carrier phase params_tracker_gnss_->gnss_opt); - //WOLF_DEBUG("preprocess: filtered observations: ", inc_snapshot->getObservations()->size()); - // compute corrected Ranges inc_snapshot->computeRanges(fix_incoming_.sat_azel, fix_incoming_.lat_lon, @@ -111,17 +114,23 @@ void ProcessorTrackerGnss::preProcess() } #ifdef _WOLF_DEBUG - std::string discarded_str, detected_str; + std::string used_rtklib_str, discarded_rtklib_str, detected_str, discarded_gnssutils_str; + for (auto sat : fix_incoming_.used_sats) + used_rtklib_str += std::to_string(sat) + " "; for (auto sat : fix_incoming_.discarded_sats) - discarded_str += std::to_string(sat) + " "; + discarded_rtklib_str += std::to_string(sat) + " "; for (auto ftr_pair : untracked_incoming_features_) detected_str += std::to_string(ftr_pair.first) + " "; + for (auto sat_disc : discarded_gnssutils) + if (fix_incoming_.discarded_sats.count(sat_disc) == 0) + discarded_gnssutils_str += std::to_string(sat_disc) + " "; #endif WOLF_DEBUG("ProcessorTrackerGnss::preProcess()", - "\n\tinitial observations: ", n_initial, - "\n\tRTKLIB discarded: ", fix_incoming_.discarded_sats.size(), " (", discarded_str, ")", - "\n\tgnssutils discarded: ", n_initial - untracked_incoming_features_.size() - fix_incoming_.discarded_sats.size(), + "\n\tinitial observations: ", n_initial, " (", initial_str, ")", + "\n\tRTKLIB used: ", fix_incoming_.used_sats.size(), " (", used_rtklib_str, ")", + //"\n\tRTKLIB discarded: ", fix_incoming_.discarded_sats.size(), " (", discarded_rtklib_str, ")", + "\n\tgnssutils discarded: ", n_initial - untracked_incoming_features_.size() - fix_incoming_.discarded_sats.size(), " (", discarded_gnssutils_str, ")", "\n\tdetected incoming features: ", untracked_incoming_features_.size(), " (", detected_str, ")"); } @@ -616,8 +625,8 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas // discard if residual too high evaluated at the current estimation if (std::abs(residual) > params_tracker_gnss_->outlier_residual_th) { - //WOLF_WARN("Discarding FactorGnssPseudoRange, considered OUTLIER"); - //WOLF_TRACE("Feature: ", fac->getMeasurement(),"\nError: ",fac->getMeasurementSquareRootInformationUpper().inverse()*residual); + WOLF_WARN("Discarding FactorGnssPseudoRange, considered OUTLIER"); + WOLF_TRACE("Feature: ", fac->getMeasurement(),"\nError: ",fac->getMeasurementSquareRootInformationUpper().inverse()*residual); remove_fac.push_back(fac_pr); // store for statistics outliers_pseudorange_++; @@ -650,8 +659,8 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas // discard if residual too high evaluated at the current estimation if (std::abs(residual) > params_tracker_gnss_->outlier_residual_th) { - //WOLF_WARN("Discarding FactorGnssPseudoRange, considered OUTLIER"); - //WOLF_TRACE("Residual: ", residual,"\nError: ",fac->getMeasurementSquareRootInformationUpper().inverse()*residual); + WOLF_WARN("Discarding FactorGnssTdcp, considered OUTLIER"); + WOLF_TRACE("Residual: ", residual,"\nError: ",fac->getMeasurementSquareRootInformationUpper().inverse()*residual); remove_fac.push_back(fac_tdcp); // store for statistics outliers_tdcp_++; @@ -671,7 +680,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas //assert(false); fac->remove(); } - WOLF_DEBUG("ProcessorTrackerGnss::removeOutliers:", + WOLF_INFO("ProcessorTrackerGnss::removeOutliers:", "\n\tPseudorange: ", outliers_pseudorange_, "\t( ", (100.0 * outliers_pseudorange_)/(outliers_pseudorange_+inliers_pseudorange_), " %)"); if (params_tracker_gnss_->gnss_opt.tdcp.enabled and params_tracker_gnss_->remove_outliers_tdcp) std::cout << "\tTDCP: "