wip

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;

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:        "