Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
G
gnss
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Deploy
Releases
Model registry
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
mobile_robotics
wolf_projects
wolf_lib
plugins
gnss
Commits
40215027
Commit
40215027
authored
4 years ago
by
Joan Vallvé Navarro
Browse files
Options
Downloads
Patches
Plain Diff
wip
parent
b261e4e7
No related branches found
No related tags found
1 merge request
!21
Draft: Resolve "tracking carrier phase detecting cycle slips"
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
include/gnss/processor/processor_tracker_gnss.h
+2
-0
2 additions, 0 deletions
include/gnss/processor/processor_tracker_gnss.h
src/processor/processor_tracker_gnss.cpp
+31
-13
31 additions, 13 deletions
src/processor/processor_tracker_gnss.cpp
with
33 additions
and
13 deletions
include/gnss/processor/processor_tracker_gnss.h
+
2
−
0
View file @
40215027
...
...
@@ -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)
...
...
This diff is collapsed.
Click to expand it.
src/processor/processor_tracker_gnss.cpp
+
31
−
13
View file @
40215027
...
...
@@ -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
(
"
\t
not 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
(
"
\t
tracked: "
,
feat_in
->
trackId
()
,
" last: "
,
feat_in
->
id
()
,
" inc: "
,
ftr
->
id
()
,
" !"
);
WOLF_DEBUG
(
"
\t
tracked: "
,
feat_in
->
trackId
()
,
" last: "
,
feat_in
->
id
()
,
" inc: "
,
ftr
->
id
()
,
" !"
);
}
else
{
wrong_carrier
++
;
WOLF_DEBUG
(
"
\t
not tracked, wrong carrier phase value"
);
}
}
else
{
WOLF_DEBUG_COND
(
untracked_incoming_features_
.
count
(
sat_num
)
==
0
,
"
\t
not tracked, missing satellite"
);
WOLF_DEBUG_COND
(
untracked_incoming_features_
.
count
(
sat_num
)
==
1
,
"
\t
not tracked, wrong carrier phase value"
);
}
WOLF_DEBUG
(
"
\t
not 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
(),
")"
);
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment