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

wip

parent b261e4e7
No related branches found
No related tags found
1 merge request!21Draft: Resolve "tracking carrier phase detecting cycle slips"
......@@ -25,6 +25,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
bool init_frames, pseudo_ranges, fix;
double enu_map_fix_dist;
int min_sbas_sats;
bool detect_cycle_slips;
ParamsProcessorTrackerGnss() = default;
ParamsProcessorTrackerGnss(std::string _unique_name, const ParamsServer& _server):
......@@ -39,6 +40,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
fix = _server.getParam<bool> (prefix + _unique_name + "/fix");
pseudo_ranges = _server.getParam<bool> (prefix + _unique_name + "/pseudo_ranges");
min_sbas_sats = _server.getParam<int> (prefix + _unique_name + "/gnss/min_sbas_sats");
detect_cycle_slips = _server.getParam<bool> (prefix + _unique_name + "/detect_cycle_slips");
// GNSS OPTIONS (see rtklib.h)
gnss_opt.sateph = _server.getParam<int> (prefix + _unique_name + "/gnss/sateph"); // satellite ephemeris option: EPHOPT_BRDC(0):broadcast ephemeris, EPHOPT_PREC(1): precise ephemeris, EPHOPT_SBAS(2): broadcast + SBAS, EPHOPT_SSRAPC(3): broadcast + SSR_APC, EPHOPT_SSRCOM(4): broadcast + SSR_COM, EPHOPT_LEX(5): QZSS LEX ephemeris, EPHOPT_SBAS2(6):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), EPHOPT_SBAS3(7):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_SBAS2), EPHOPT_SBAS4(8):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_BRDC)
......
......@@ -214,6 +214,8 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat
assert(_capture == incoming_ptr_);
int common_sats = 0;
int wrong_carrier = 0;
int cycle_slips = 0;
for (auto feat_in : _features_in)
{
......@@ -224,29 +226,45 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat
WOLF_DEBUG("tracking " , feat_in->trackId() , ", sat number ", sat_num);
// satellite exists
if (untracked_incoming_features_.count(sat_num) != 0)
{
common_sats++;
if (untracked_incoming_features_.count(sat_num) != 0 and
std::abs(untracked_incoming_features_.at(sat_num)->getObservation().L[0]) > 1e-12) // Track only carrier phase valid
{
auto ftr = untracked_incoming_features_[sat_num];
untracked_incoming_features_.erase(sat_num);
_features_out.push_back(ftr);
_feature_correspondences[ftr] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
// valid carrier phase data
if (std::abs(untracked_incoming_features_.at(sat_num)->getObservation().L[0]) > 1e-12)
{
// cycle slip
if (params_tracker_gnss_->detect_cycle_slips and
std::abs(untracked_incoming_features_.at(sat_num)->getRange().L - feat_in_gnss->getRange().L) > 0.1)
{
cycle_slips++;
WOLF_DEBUG("\tnot tracked, cycle slip detected" );
continue;
}
auto ftr = untracked_incoming_features_[sat_num];
untracked_incoming_features_.erase(sat_num);
_features_out.push_back(ftr);
_feature_correspondences[ftr] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
WOLF_DEBUG("\ttracked: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" );
WOLF_DEBUG("\ttracked: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" );
}
else
{
wrong_carrier++;
WOLF_DEBUG("\tnot tracked, wrong carrier phase value" );
}
}
else
{
WOLF_DEBUG_COND(untracked_incoming_features_.count(sat_num) == 0, "\tnot tracked, missing satellite" );
WOLF_DEBUG_COND(untracked_incoming_features_.count(sat_num) == 1, "\tnot tracked, wrong carrier phase value" );
}
WOLF_DEBUG("\tnot tracked, missing satellite" );
}
WOLF_WARN_COND(_features_out.empty() and
_features_out.begin() == known_features_incoming_.begin(),
"ProcessorTrackerGnss::trackFeatures: LOST TRACK OF ALL SATELLITES of ", _features_in.size(),
" - ", common_sats, " due to wrong Carrier Phase data.");
" - common sats: ", common_sats,
" - wrong carrier data: ", wrong_carrier,
" - cycle slips: ", cycle_slips);
WOLF_DEBUG_COND(_features_out.begin() == new_features_incoming_.begin(),
"ProcessorTrackerGnss::trackFeatures: tracked " ,
_features_out.size(), " new features (of ", _features_in.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