diff --git a/include/gnss/processor/processor_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h
index ca6eb0de5bd92cda2bdd2b39952a01fcfabe4e2c..b8ee8b385adb6c6e736d416b4a21f4949e0759e0 100644
--- a/include/gnss/processor/processor_tracker_gnss.h
+++ b/include/gnss/processor/processor_tracker_gnss.h
@@ -17,7 +17,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
GnssUtils::Options gnss_opt;
GnssUtils::Options fix_opt{GnssUtils::default_options};
double max_time_span;
- bool remove_outliers;
+ bool remove_outliers, remove_outliers_tdcp;
double outlier_residual_th;
bool init_frames;
@@ -52,18 +52,19 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
gnss_opt.tdcp.enabled = _server.getParam<bool>(prefix + _unique_name + "/gnss/tdcp/enabled");
if (gnss_opt.tdcp.enabled)
{
- gnss_opt.tdcp.corr_iono = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_iono");
- gnss_opt.tdcp.corr_tropo = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_tropo");
- gnss_opt.tdcp.loss_function = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/loss_function");
- gnss_opt.tdcp.time_window = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/time_window");
- gnss_opt.tdcp.sigma_atm = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/sigma_atm");
- gnss_opt.tdcp.sigma_carrier = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/sigma_carrier");
- gnss_opt.tdcp.multi_freq = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/multi_freq");
+ remove_outliers_tdcp = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/remove_outliers");
+ gnss_opt.tdcp.corr_iono = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_iono");
+ gnss_opt.tdcp.corr_tropo = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_tropo");
+ gnss_opt.tdcp.loss_function = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/loss_function");
+ gnss_opt.tdcp.time_window = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/time_window");
+ gnss_opt.tdcp.sigma_atm = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/sigma_atm");
+ gnss_opt.tdcp.sigma_carrier = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/sigma_carrier");
+ gnss_opt.tdcp.multi_freq = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/multi_freq");
}
// COMPUTE FIX OPTIONS (RAIM)
fix_opt.elmin = 0;
- fix_opt.maxgdop = 10;
+ fix_opt.maxgdop = 30;
}
std::string print() const
@@ -121,6 +122,8 @@ class ProcessorTrackerGnss : public ProcessorTrackerFeature
SensorGnssPtr sensor_gnss_;
std::map<int, FeatureGnssSatellitePtr> untracked_incoming_features_, untracked_last_features_;
GnssUtils::ComputePosOutput fix_incoming_, fix_last_;
+ unsigned int outliers_pseudorange_, outliers_tdcp_, inliers_pseudorange_, inliers_tdcp_;
+ std::map<int, unsigned int> sat_outliers_;
/** \brief Track provided features in \b _capture
* \param _features_in input list of features in \b last to track
@@ -212,7 +215,7 @@ class ProcessorTrackerGnss : public ProcessorTrackerFeature
void advanceDerived() override;
void resetDerived() override;
- void removeOutliers(FactorBasePtrList fac_list, CaptureBasePtr cap) const;
+ void removeOutliers(FactorBasePtrList fac_list, CaptureBasePtr cap);
};
inline ProcessorTrackerGnss::~ProcessorTrackerGnss()
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index 67389a4d2efdd058e5324ffee666a840a251478c..35be5420d280579442d2aef6d97a18233e7474d9 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -10,7 +10,11 @@ namespace wolf
ProcessorTrackerGnss::ProcessorTrackerGnss(ParamsProcessorTrackerGnssPtr _params_tracker_gnss) :
ProcessorTrackerFeature("ProcessorTrackerGnss", 3, _params_tracker_gnss),
- params_tracker_gnss_(_params_tracker_gnss)
+ params_tracker_gnss_(_params_tracker_gnss),
+ outliers_pseudorange_(0),
+ outliers_tdcp_(0),
+ inliers_pseudorange_(0),
+ inliers_tdcp_(0)
{
}
@@ -20,7 +24,7 @@ void ProcessorTrackerGnss::preProcess()
GnssUtils::SnapshotPtr inc_snapshot = std::static_pointer_cast<CaptureGnss>(incoming_ptr_)->getSnapshot();
- WOLF_DEBUG("preprocess: initial observations: ", inc_snapshot->getObservations()->size());
+ WOLF_INFO("preprocess: initial observations: ", inc_snapshot->getObservations()->size());
// compute satellites positions
if (!inc_snapshot ->satellitesComputed())
@@ -39,13 +43,13 @@ void ProcessorTrackerGnss::preProcess()
// Set ECEF-ENU
if (!sensor_gnss_->isEnuDefined() and sensor_gnss_->isEnuModeAuto() and fix_incoming_.stat != 0)
{
- WOLF_DEBUG("setting ECEF-ENU: ", fix_incoming_.pos.transpose());
+ WOLF_INFO("setting ECEF-ENU: ", fix_incoming_.pos.transpose());
sensor_gnss_->setEcefEnu(fix_incoming_.pos, true);
}
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());
+ WOLF_INFO("preprocess: RTKLIB excluded observations: ", fix_incoming_.discarded_sats.size());
// filter observations (available ephemeris, constellations and elevation&SNR)
inc_snapshot->filterObservations(fix_incoming_.discarded_sats, // discarded sats
fix_incoming_.sat_azel,
@@ -53,7 +57,7 @@ void ProcessorTrackerGnss::preProcess()
false, // check carrier phase
params_tracker_gnss_->gnss_opt);
- WOLF_DEBUG("preprocess: filtered observations: ", inc_snapshot->getObservations()->size());
+ WOLF_INFO("preprocess: filtered observations: ", inc_snapshot->getObservations()->size());
// compute corrected Ranges
inc_snapshot->computeRanges(fix_incoming_.sat_azel,
@@ -71,7 +75,7 @@ void ProcessorTrackerGnss::preProcess()
untracked_incoming_features_[feat->satNumber()] = feat;
}
- WOLF_DEBUG("preprocess: untracked incoming features: ", untracked_incoming_features_.size());
+ WOLF_INFO("preprocess: untracked incoming features: ", untracked_incoming_features_.size());
}
unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _features_in,
@@ -79,7 +83,10 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat
FeatureBasePtrList& _features_out,
FeatureMatchMap& _feature_correspondences)
{
- WOLF_DEBUG("tracking " , _features_in.size() , " features...");
+ if (_features_in.empty())
+ return 0;
+
+ WOLF_INFO("tracking " , _features_in.size() , " features...");
assert(_capture == incoming_ptr_);
@@ -100,27 +107,28 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat
}
}
- WOLF_DEBUG("Tracked features: " , _features_out.size());
+ WOLF_INFO("Tracked features: " , _features_out.size());
return _features_out.size();
}
bool ProcessorTrackerGnss::voteForKeyFrame() const
{
- //WOLF_DEBUG("ProcessorTrackerGnss::voteForKeyFrame");
+ //WOLF_INFO("ProcessorTrackerGnss::voteForKeyFrame");
// too old origin
if (origin_ptr_== nullptr or (last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp()) > params_tracker_gnss_->max_time_span )
{
- WOLF_DEBUG( "Vote for KF because of time span or null origin" );
+ WOLF_INFO( "Vote for KF because of time span or null origin" );
return true;
}
// known features
WOLF_DEBUG("Nbr. of active feature tracks: " , known_features_incoming_.size() );
- if (known_features_incoming_.size() < params_tracker_feature_->min_features_for_keyframe)
+ if (known_features_incoming_.size() < params_tracker_feature_->min_features_for_keyframe
+ and !untracked_last_features_.empty())
{
- WOLF_DEBUG( "Vote for KF because of too less known_features_incoming" );
+ WOLF_INFO( "Vote for KF because of too less known_features_incoming and not empty untracked in last" );
return true;
}
@@ -185,7 +193,7 @@ void ProcessorTrackerGnss::establishFactors()
last_ptr_->addStateBlock("TG", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(1)), true), getProblem());
last_ptr_->addStateBlock("TE", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(2)), true), getProblem());
last_ptr_->addStateBlock("TC", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(3)), true), getProblem());
- //WOLF_DEBUG("last clock bias set: ", last_ptr_->getStateBlock("T")->getState());
+ //WOLF_INFO("last clock bias set: ", last_ptr_->getStateBlock("T")->getState());
}
// Initialize clock bias stateblocks in capture
else if (!last_clock_bias_init)
@@ -194,7 +202,7 @@ void ProcessorTrackerGnss::establishFactors()
last_ptr_->getStateBlock("TG")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(1)));
last_ptr_->getStateBlock("TE")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(2)));
last_ptr_->getStateBlock("TC")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(3)));
- //WOLF_DEBUG("last clock bias initialized: ", last_ptr_->getStateBlock("T")->getState());
+ //WOLF_INFO("last clock bias initialized: ", last_ptr_->getStateBlock("T")->getState());
last_clock_bias_init = true;
}
@@ -241,14 +249,21 @@ void ProcessorTrackerGnss::establishFactors()
auto ftr_r = std::dynamic_pointer_cast<FeatureGnssSatellite>(ts_ftr_r_it->second);
assert(ftr_r != nullptr);
+ // dt
+ double dt = ftr_k->getCapture()->getTimeStamp() - ts_ftr_r_it->first;
+
// discard incomming-last and last-last
- if (ftr_k->getCapture()->getTimeStamp() < ts_ftr_r_it->first or ftr_k == ftr_r)
+ if (dt < 0 or ftr_k == ftr_r)
continue;
// within time window
- if (ftr_k->getCapture()->getTimeStamp() - ts_ftr_r_it->first > params_tracker_gnss_->gnss_opt.tdcp.time_window)
+ if (dt > params_tracker_gnss_->gnss_opt.tdcp.time_window)
break;
+ // discard removing Frame/capture/feature
+ if (ftr_r->isRemoving() or ftr_r->getCapture()->isRemoving() or ftr_r->getCapture()->getFrame()->isRemoving())
+ continue;
+
// check valid measurement
if (ftr_r->getObservation().L[0] < 1e-12) // TODO: move to GnssUtils::isValid(double, Combination)
continue;
@@ -266,8 +281,9 @@ void ProcessorTrackerGnss::establishFactors()
}
// emplace tdcp factor
+ double var_tdcp = dt * std::pow(params_tracker_gnss_->gnss_opt.tdcp.sigma_atm,2) + std::pow(params_tracker_gnss_->gnss_opt.tdcp.sigma_carrier,2);
auto new_fac = FactorBase::emplace<FactorGnssTdcp>(ftr_k,
- 0.1, // TODO: compute from params and dt
+ sqrt(var_tdcp),
ftr_r,
ftr_k,
sensor_gnss_,
@@ -275,7 +291,7 @@ void ProcessorTrackerGnss::establishFactors()
params_tracker_gnss_->gnss_opt.tdcp.loss_function);
new_factors.push_back(new_fac);
- // WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
+ // WOLF_INFO( "Factor: track: " , feature_in_last->trackId(),
// " origin: " , feature_in_origin->id() ,
// " from last: " , feature_in_last->id() );
}
@@ -284,7 +300,7 @@ void ProcessorTrackerGnss::establishFactors()
}
// remove outliers
- if (!new_factors.empty() and params_tracker_gnss_->remove_outliers)
+ if (!new_factors.empty() and (params_tracker_gnss_->remove_outliers or params_tracker_gnss_->remove_outliers_tdcp))
removeOutliers(new_factors, last_ptr_);
//getProblem()->print(4, 1, 1, 1);
@@ -313,32 +329,77 @@ void ProcessorTrackerGnss::postProcess()
}
-void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBasePtr cap) const
+void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBasePtr cap)
{
WOLF_DEBUG("ProcessorTrackerGnss::removeOutliers");
FactorBasePtrList remove_fac;
- //WOLF_DEBUG( "PR ", getName()," rejectOutlier...");
- // copy states
- Eigen::Vector3d x(cap->getFrame()->getP()->getState());
- Eigen::Vector4d o(cap->getFrame()->getO()->getState());
+ //WOLF_INFO( "PR ", getName()," rejectOutlier...");
+
+ // PseudoRange states
+ Eigen::Vector3d x;
+ Eigen::Vector4d o;
+ Eigen::Vector1d clock_bias;
+ Eigen::Vector1d clock_bias_glo;
+ Eigen::Vector1d clock_bias_gal;
+ Eigen::Vector1d clock_bias_cmp;
+ Eigen::Vector3d x_antena_pr;
+
+
+ // state for pseudoranges is fix solution if OK
+ if (cap == last_ptr_ and fix_last_.stat != 0)
+ {
+ WOLF_DEBUG("OUTLIERS COMPUTED USING fix_last");
+ x = sensor_gnss_->getREnuMap().transpose() * (sensor_gnss_->getREnuEcef() * fix_last_.pos + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap());
+ o << 0,0,0,1;
+ clock_bias << CLIGHT * fix_last_.rcv_bias(0);
+ clock_bias_glo << CLIGHT * fix_last_.rcv_bias(1);
+ clock_bias_gal << CLIGHT * fix_last_.rcv_bias(2);
+ clock_bias_cmp << CLIGHT * fix_last_.rcv_bias(3);
+ x_antena_pr = Eigen::Vector3d::Zero();
+
+ //std::cout << "x: " << x.transpose() << std::endl;
+ //std::cout << "o: " << o.transpose() << std::endl;
+ //std::cout << "clock_bias: " << clock_bias << std::endl;
+ //std::cout << "clock_bias_glo: " << clock_bias_glo << std::endl;
+ //std::cout << "clock_bias_gal: " << clock_bias_gal << std::endl;
+ //std::cout << "clock_bias_cmp: " << clock_bias_cmp << std::endl;
+ //std::cout << "x_antena_pr: " << x_antena_pr.transpose() << std::endl;
+ //std::cout << "frame p: " << cap->getFrame()->getP()->getState().transpose() << std::endl;
+ //std::cout << "frame o: " << cap->getFrame()->getO()->getState().transpose() << std::endl;
+ //std::cout << "sb clock: " << cap->getStateBlock("T")->getState() << std::endl;
+ //std::cout << "sb clock glo: " << cap->getStateBlock("TG")->getState() << std::endl;
+ //std::cout << "sb clock gal: " << cap->getStateBlock("TE")->getState() << std::endl;
+ //std::cout << "sb clock cmp: " << cap->getStateBlock("TC")->getState() << std::endl;
+ //std::cout << "sb antena: " << sensor_gnss_->getP()->getState().transpose() << std::endl;
+ }
+ else
+ {
+ x = cap->getFrame()->getP()->getState();
+ o = cap->getFrame()->getO()->getState();
+ clock_bias = cap->getStateBlock("T")->getState();
+ clock_bias_glo = cap->getStateBlock("TG")->getState();
+ clock_bias_gal = cap->getStateBlock("TE")->getState();
+ clock_bias_cmp = cap->getStateBlock("TC")->getState();
+ x_antena_pr = sensor_gnss_->getP()->getState();
+ }
+
+ // TDCP states
+ Eigen::Vector3d x_k(cap->getFrame()->getP()->getState());
+ Eigen::Vector4d o_k(cap->getFrame()->getO()->getState());
Eigen::Vector3d x_r(cap->getFrame()->getP()->getState());
Eigen::Vector4d o_r(cap->getFrame()->getO()->getState());
- Eigen::Vector1d clock_bias(cap->getStateBlock("T")->getState());
- Eigen::Vector1d clock_bias_glo(cap->getStateBlock("TG")->getState());
- Eigen::Vector1d clock_bias_gal(cap->getStateBlock("TE")->getState());
- Eigen::Vector1d clock_bias_cmp(cap->getStateBlock("TC")->getState());
+ Eigen::Vector1d clock_bias_k(cap->getStateBlock("T")->getState());
+ Eigen::Vector1d clock_bias_r(cap->getStateBlock("T")->getState());
Eigen::Vector3d x_antena(sensor_gnss_->getP()->getState());
+
+ // Common states
Eigen::Vector3d t_ENU_map(sensor_gnss_->getEnuMapTranslation()->getState());
Eigen::Vector1d roll_ENU_map(sensor_gnss_->getEnuMapRoll()->getState());
Eigen::Vector1d pitch_ENU_map(sensor_gnss_->getEnuMapPitch()->getState());
Eigen::Vector1d yaw_ENU_map(sensor_gnss_->getEnuMapYaw()->getState());
- //std::cout << "Frame p: " << x.transpose() << std::endl;
- //std::cout << "Frame o: " << o.transpose() << std::endl;
- //std::cout << "clock_bias: " << clock_bias << std::endl;
- //std::cout << "x_antena: " << x_antena.transpose() << std::endl;
//std::cout << "t_ENU_map: " << t_ENU_map.transpose() << std::endl;
//std::cout << "roll_ENU_map: " << roll_ENU_map << std::endl;
//std::cout << "pitch_ENU_map: " << pitch_ENU_map << std::endl;
@@ -349,7 +410,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
o.data(),
clock_bias.data(),
clock_bias_glo.data(),
- x_antena.data(),
+ x_antena_pr.data(),
t_ENU_map.data(),
roll_ENU_map.data(),
pitch_ENU_map.data(),
@@ -358,7 +419,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
o.data(),
clock_bias.data(),
clock_bias_gal.data(),
- x_antena.data(),
+ x_antena_pr.data(),
t_ENU_map.data(),
roll_ENU_map.data(),
pitch_ENU_map.data(),
@@ -367,17 +428,17 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
o.data(),
clock_bias.data(),
clock_bias_cmp.data(),
- x_antena.data(),
+ x_antena_pr.data(),
t_ENU_map.data(),
roll_ENU_map.data(),
pitch_ENU_map.data(),
yaw_ENU_map.data()};
double* parameters_tdcp[11] = {x_r.data(),
o_r.data(),
- clock_bias.data(),
- x.data(),
- o.data(),
- clock_bias.data(),
+ clock_bias_r.data(),
+ x_k.data(),
+ o_k.data(),
+ clock_bias_k.data(),
x_antena.data(),
t_ENU_map.data(),
roll_ENU_map.data(),
@@ -391,10 +452,13 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
{
// PSEUDO RANGE FACTORS
auto fac_pr = std::dynamic_pointer_cast<FactorGnssPseudoRange>(fac);
+ auto ftr_sat = std::static_pointer_cast<FeatureGnssSatellite>(fac->getFeature());
+ int sat = ftr_sat->satNumber();
+
if (fac_pr)
{
// evaluate the factor in this state
- switch (std::static_pointer_cast<FeatureGnssSatellite>(fac->getFeature())->getSatellite().sys)
+ switch (ftr_sat->getSatellite().sys)
{
case SYS_GLO:
fac_pr->evaluate(parameters_glo_pr, &residual, nullptr);
@@ -410,53 +474,82 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
break;
}
-
// RTKLIB FIX error
//int sat = std::static_pointer_cast<FeatureGnssSatellite>(fac->getFeature())->getSatellite().sat;
//assert(fix_last_.prange_residuals.count(sat) && "sat not used when computing fix!");
- //WOLF_DEBUG("FactorGnssPseudoRange error = ", fac->getMeasurementSquareRootInformationUpper().inverse()*residual);
- //WOLF_DEBUG("RTKLIB pntpos error = ", fix_last_.prange_residuals.at(sat));
+ //WOLF_INFO("FactorGnssPseudoRange error = ", fac->getMeasurementSquareRootInformationUpper().inverse()*residual);
+ //WOLF_INFO("RTKLIB pntpos error = ", fix_last_.prange_residuals.at(sat));
// 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_++;
+ if (not sat_outliers_.count(sat))
+ sat_outliers_.emplace(sat, 1);
+ else
+ sat_outliers_[sat]++;
+
}
+ else
+ inliers_pseudorange_++;
}
// TDCP FACTORS
auto fac_tdcp = std::dynamic_pointer_cast<FactorGnssTdcp>(fac);
- if (fac_tdcp)
+ if (fac_tdcp and params_tracker_gnss_->remove_outliers_tdcp)
{
// update ref frame
x_r = fac_tdcp->getCaptureOther()->getFrame()->getP()->getState();
o_r = fac_tdcp->getCaptureOther()->getFrame()->getO()->getState();
+ clock_bias_r = fac_tdcp->getCaptureOther()->getStateBlock("T")->getState();
parameters_tdcp[0] = x_r.data();
parameters_tdcp[1] = o_r.data();
+ parameters_tdcp[2] = clock_bias_r.data();
// evaluate
fac_tdcp->evaluate(parameters_tdcp, &residual, nullptr);
- WOLF_DEBUG("FactorGnssTdcp with residual = ", residual);
+ //WOLF_INFO("FactorGnssTdcp with residual = ", residual);
// 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 FactorGnssPseudoRange, considered OUTLIER");
+ //WOLF_TRACE("Residual: ", residual,"\nError: ",fac->getMeasurementSquareRootInformationUpper().inverse()*residual);
remove_fac.push_back(fac_tdcp);
+ // store for statistics
+ outliers_tdcp_++;
+ if (not sat_outliers_.count(sat))
+ sat_outliers_.emplace(sat, 1);
+ else
+ sat_outliers_[sat]++;
}
+ else
+ inliers_tdcp_++;
}
}
// remove outliers
for (auto fac : remove_fac)
{
- assert(false);
+ //assert(false);
fac->remove();
}
- WOLF_DEBUG("ProcessorTrackerGnss::removeOutliers done");
+ WOLF_INFO("ProcessorTrackerGnss::removeOutliers:",
+ "\n\tPseudorange: ", outliers_pseudorange_, "\t( ", (100.0 * outliers_pseudorange_)/(outliers_pseudorange_+inliers_pseudorange_), " %)",
+ "\n\tTDCP: ", outliers_tdcp_, "\t( ", (100.0 * outliers_tdcp_)/(outliers_tdcp_+inliers_tdcp_), " %)",
+ "\n\tsats:");
+ std::string sat_out_str("");
+ for (auto sat_out : sat_outliers_)
+ {
+ const int& sat = sat_out.first;
+ int sys = satsys(sat,NULL);
+ const unsigned int& outliers = sat_out.second;
+ std::cout << "\t\tsat " << sat << "(" << sys << "): " << outliers << std::endl;
+ }
}
} // namespace wolf