Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
gnss_utils
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD 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
labrobotica
algorithms
gnss_utils
Commits
faa20fdd
Commit
faa20fdd
authored
4 years ago
by
Joan Vallvé Navarro
Browse files
Options
Downloads
Patches
Plain Diff
WIP
parent
db78648f
No related branches found
No related tags found
3 merge requests
!20
new tag
,
!19
new tag
,
!17
Resolve "TDCP batch implementation"
Changes
4
Hide whitespace changes
Inline
Side-by-side
Showing
4 changed files
deps/RTKLIB
+1
-1
1 addition, 1 deletion
deps/RTKLIB
include/gnss_utils/tdcp.h
+13
-0
13 additions, 0 deletions
include/gnss_utils/tdcp.h
include/gnss_utils/utils/satellite.h
+18
-0
18 additions, 0 deletions
include/gnss_utils/utils/satellite.h
src/tdcp.cpp
+61
-51
61 additions, 51 deletions
src/tdcp.cpp
with
93 additions
and
52 deletions
RTKLIB
@
4ab9a199
Compare
0260d919
...
4ab9a199
Subproject commit
0260d91932df0ca0691842aa99b39acde5d49c45
Subproject commit
4ab9a199ff46b1220fb4fe99b019c8df526e53e9
This diff is collapsed.
Click to expand it.
include/gnss_utils/tdcp.h
+
13
−
0
View file @
faa20fdd
...
...
@@ -48,6 +48,19 @@ bool Tdcp(SnapshotPtr snapshot_r,
double
&
residual
,
std
::
set
<
int
>
raim_discarded_sats
,
const
TdcpBatchParams
&
tdcp_params
);
bool
Tdcp
(
const
Eigen
::
Vector3d
&
x_r
,
Eigen
::
Matrix
<
double
,
Eigen
::
Dynamic
,
4
>&
A
,
Eigen
::
VectorXd
&
r
,
Eigen
::
VectorXd
&
drho_m
,
Eigen
::
Matrix
<
double
,
3
,
Eigen
::
Dynamic
>&
s_k
,
Eigen
::
Matrix
<
double
,
3
,
Eigen
::
Dynamic
>&
s_r
,
Eigen
::
Vector4d
&
d
,
Eigen
::
Matrix4d
&
cov_d
,
double
&
residual
,
std
::
set
<
int
>
raim_discarded_sats
,
const
TdcpBatchParams
&
tdcp_params
);
}
// namespace GnssUtils
#endif
/* INCLUDE_GNSS_UTILS_Tdcp_H_ */
This diff is collapsed.
Click to expand it.
include/gnss_utils/utils/satellite.h
+
18
−
0
View file @
faa20fdd
...
...
@@ -25,6 +25,24 @@ namespace GnssUtils
const
Navigation
&
nav
,
const
int
&
eph_opt
);
// see rtklib.h L396
Eigen
::
Vector2d
computeAzel
(
const
Satellite
&
sat
,
const
Eigen
::
Vector3d
&
receiver_ecef
)
{
Eigen
::
Vector3d
unit_vector
;
geodist
(
sat
.
pose
.
data
(),
receiver_ecef
.
data
(),
unit_vector
.
data
());
Eigen
::
Vector2d
azel
;
Eigen
::
Vector3d
pos_enu
;
// TODO
satazel
(
pos_enu
.
data
(),
unit_vector
.
data
(),
azel
.
data
());
}
Azels
computeAzels
(
const
Satellites
&
sats
,
const
Eigen
::
Vector3d
&
receiver_ecef
)
{
Azels
azels
;
for
(
auto
sat
:
sats
)
azels
.
insert
(
sat
.
first
,
computeAzel
(
sat
,
receiver_ecef
));
return
azels
;
}
struct
Satellite
{
int
sys
;
...
...
This diff is collapsed.
Click to expand it.
src/tdcp.cpp
+
61
−
51
View file @
faa20fdd
...
...
@@ -149,6 +149,8 @@ bool Tdcp(SnapshotPtr snapshot_r,
{
tr
=
obs_r
.
time
.
time
+
obs_r
.
time
.
sec
;
tk
=
obs_k
.
time
.
time
+
obs_k
.
time
.
sec
;
std
::
cout
<<
"tr: "
<<
tr
<<
std
::
endl
;
std
::
cout
<<
"tk: "
<<
tk
<<
std
::
endl
;
}
// Carrier phase
...
...
@@ -171,12 +173,9 @@ bool Tdcp(SnapshotPtr snapshot_r,
}
int
n_differences
=
row_2_sat_freq
.
size
();
// Initial guess
Eigen
::
Vector4d
d_0
(
d
),
delta_d
(
Eigen
::
Vector4d
::
Zero
());
#if GNSS_UTILS_TDCP_DEBUG == 1
std
::
cout
<<
"Receiver at t_r: "
<<
x_r
.
transpose
()
<<
std
::
endl
;
std
::
cout
<<
"d initial guess: "
<<
d
_0
.
transpose
()
<<
std
::
endl
;
std
::
cout
<<
"d initial guess: "
<<
d
.
transpose
()
<<
std
::
endl
;
std
::
cout
<<
"common sats: "
;
for
(
auto
row_sat_freq_it
:
row_2_sat_freq
)
std
::
cout
<<
row_sat_freq_it
.
second
.
first
<<
" "
;
...
...
@@ -189,7 +188,6 @@ bool Tdcp(SnapshotPtr snapshot_r,
Eigen
::
VectorXd
drho_m
(
Eigen
::
VectorXd
::
Zero
(
n_differences
));
Eigen
::
Matrix
<
double
,
3
,
Eigen
::
Dynamic
>
s_k
(
Eigen
::
Matrix
<
double
,
3
,
Eigen
::
Dynamic
>::
Zero
(
3
,
n_differences
));
Eigen
::
Matrix
<
double
,
3
,
Eigen
::
Dynamic
>
s_r
(
Eigen
::
Matrix
<
double
,
3
,
Eigen
::
Dynamic
>::
Zero
(
3
,
n_differences
));
int
sat_i
=
0
;
for
(
auto
row_sat_freq_it
:
row_2_sat_freq
)
{
...
...
@@ -240,6 +238,36 @@ bool Tdcp(SnapshotPtr snapshot_r,
}
// LEAST SQUARES SOLUTION =======================================================================
std
::
set
<
int
>
raim_discarded_rows
;
bool
result
=
Tdcp
(
x_r
,
A
,
r
,
drho_m
,
s_k
,
s_r
,
d
,
cov_d
,
residual
,
raim_discarded_rows
,
tdcp_params
);
for
(
auto
disc_row
:
raim_discarded_rows
)
raim_discarded_sats
.
insert
(
row_2_sat_freq
[
disc_row
].
first
);
// weight covariance with the measurement noise (proportional to time)
double
sq_sigma_Tdcp
=
(
tk
-
tr
)
*
tdcp_params
.
tdcp
.
sigma_atm
*
tdcp_params
.
tdcp
.
sigma_atm
+
2
*
tdcp_params
.
tdcp
.
sigma_carrier
*
tdcp_params
.
tdcp
.
sigma_carrier
;
cov_d
*=
sq_sigma_Tdcp
;
return
result
;
}
bool
Tdcp
(
const
Eigen
::
Vector3d
&
x_r
,
Eigen
::
Matrix
<
double
,
Eigen
::
Dynamic
,
4
>&
A
,
Eigen
::
VectorXd
&
r
,
Eigen
::
VectorXd
&
drho_m
,
Eigen
::
Matrix
<
double
,
3
,
Eigen
::
Dynamic
>&
s_k
,
Eigen
::
Matrix
<
double
,
3
,
Eigen
::
Dynamic
>&
s_r
,
Eigen
::
Vector4d
&
d
,
Eigen
::
Matrix4d
&
cov_d
,
double
&
residual
,
std
::
set
<
int
>
raim_discarded_rows
,
const
TdcpBatchParams
&
tdcp_params
)
{
// Initial guess
Eigen
::
Vector4d
d_0
(
d
),
delta_d
(
Eigen
::
Vector4d
::
Zero
());
for
(
int
j
=
0
;
j
<
tdcp_params
.
max_iterations
;
j
++
)
{
// fill A and r
...
...
@@ -301,10 +329,9 @@ bool Tdcp(SnapshotPtr snapshot_r,
int
worst_sat_row
=
-
1
;
double
best_residual
=
1e12
;
Eigen
::
Vector4d
best_d
;
int
n_removed_rows
=
1
;
// remove some satellites
while
(
raim_discarded_
sat
s
.
size
()
<
tdcp_params
.
raim_n
)
while
(
raim_discarded_
row
s
.
size
()
<
tdcp_params
.
raim_n
)
{
auto
A_raim
=
A
;
auto
r_raim
=
r
;
...
...
@@ -312,21 +339,13 @@ bool Tdcp(SnapshotPtr snapshot_r,
// solve removing each satellite
for
(
int
row_removed
=
0
;
row_removed
<
A_raim
.
rows
();
row_removed
++
)
{
int
sat_removed
=
row_2_sat_freq
.
at
(
row_removed
).
first
;
// Multi-freq: some rows for the same satellite
n_removed_rows
=
1
;
if
(
tdcp_params
.
tdcp
.
multi_freq
)
while
(
row_removed
+
n_removed_rows
<
A_raim
.
rows
()
and
row_2_sat_freq
.
at
(
row_removed
+
n_removed_rows
).
first
==
sat_removed
)
n_removed_rows
++
;
// skip already discarded rows
if
(
raim_discarded_rows
.
count
(
row_removed
)
!=
0
)
continue
;
// remove satellite rows
for
(
auto
r
=
0
;
r
<
n_removed_rows
;
r
++
)
{
A_raim
.
row
(
row_removed
+
r
)
=
Eigen
::
Vector4d
::
Zero
().
transpose
();
r_raim
(
row_removed
+
r
)
=
0
;
// not necessary
}
// remove satellite row
A_raim
.
row
(
row_removed
)
=
Eigen
::
Vector4d
::
Zero
().
transpose
();
r_raim
(
row_removed
)
=
0
;
// not necessary
// solve
Eigen
::
Vector4d
delta_d_raim
=
-
(
A_raim
.
transpose
()
*
A_raim
).
inverse
()
*
A_raim
.
transpose
()
*
r_raim
;
...
...
@@ -340,8 +359,7 @@ bool Tdcp(SnapshotPtr snapshot_r,
// residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / A_raim.rows());
#if GNSS_UTILS_TDCP_DEBUG == 1
std
::
cout
<<
"RAIM excluding sat "
<<
sat_removed
<<
" - row "
<<
row_removed
<<
"(n_rows "
<<
n_removed_rows
<<
")"
<<
std
::
endl
;
std
::
cout
<<
"RAIM excluding row "
<<
<<
row_removed
<<
std
::
endl
;
std
::
cout
<<
"Displacement vector ="
<<
d_raim
.
head
<
3
>
().
transpose
()
<<
std
::
endl
;
std
::
cout
<<
"Displacement update ="
<<
delta_d_raim
.
head
<
3
>
().
transpose
()
<<
std
::
endl
;
printf
(
"Ref distance = %7.3f m
\n
"
,
d_0
.
norm
());
...
...
@@ -363,12 +381,8 @@ bool Tdcp(SnapshotPtr snapshot_r,
}
}
// restore initial A and r
for
(
auto
row
=
0
;
row
<
n_removed_rows
;
row
++
)
{
A_raim
.
row
(
row_removed
+
row
)
=
A
.
row
(
row_removed
+
row
);
r_raim
(
row_removed
+
row
)
=
r
(
row_removed
+
row
);
}
row_removed
+=
(
n_removed_rows
-
1
);
A_raim
.
row
(
row_removed
)
=
A
.
row
(
row_removed
);
r_raim
(
row_removed
)
=
r
(
row_removed
);
}
// No successful RAIM solution
...
...
@@ -379,13 +393,9 @@ bool Tdcp(SnapshotPtr snapshot_r,
}
// store removed sat
raim_discarded_sats
.
insert
(
row_2_sat_freq
[
worst_sat_row
].
first
);
// decrease n_common_sats
n_differences
-=
n_removed_rows
;
n_common_sats
--
;
raim_discarded_rows
.
insert
(
worst_sat_row
);
// Remove selected satellite from problem
/*
// Remove selected satellite from problem
std::cout << "resizing problem..." << std::endl;
auto A_aux = A;
auto r_aux = r;
...
...
@@ -406,7 +416,7 @@ bool Tdcp(SnapshotPtr snapshot_r,
s_k.rightCols(back_elements_changing) = s_k_aux.rightCols(back_elements_changing);
r.tail(back_elements_changing) = r_aux.tail(back_elements_changing);
drho_m.tail(back_elements_changing) = drho_m_aux.tail(back_elements_changing);
}
}
*/
// apply the RAIM solution
d
=
best_d
;
...
...
@@ -415,9 +425,10 @@ bool Tdcp(SnapshotPtr snapshot_r,
#if GNSS_UTILS_TDCP_DEBUG == 1
std
::
cout
<<
"Tdcp After RAIM "
<<
std
::
endl
;
std
::
cout
<<
"
\t
Excluded sats : "
;
std
::
cout
<<
"
\t
Common sats : "
<<
n_common_sats
<<
std
::
endl
;
std
::
cout
<<
"
\t
Rows : "
<<
n_differences
<<
std
::
endl
;
std
::
cout
<<
"
\t
Excluded rows : "
;
for
(
auto
excl_row
:
raim_discarded_rows
)
std
::
cout
<<
excl_row
<<
" "
;
std
::
cout
<<
std
::
endl
;
std
::
cout
<<
"
\t
Initial guess = "
<<
d_0
.
transpose
()
<<
" ("
<<
d_0
.
head
<
3
>
().
norm
()
<<
"m)"
<<
std
::
endl
;
std
::
cout
<<
"
\t
Solution = "
<<
d
.
transpose
()
<<
" ("
<<
d
.
head
<
3
>
().
norm
()
<<
"m)"
<<
std
::
endl
;
std
::
cout
<<
"
\t
Clock offset = "
<<
d
(
3
)
<<
std
::
endl
;
...
...
@@ -430,9 +441,10 @@ bool Tdcp(SnapshotPtr snapshot_r,
#if GNSS_UTILS_TDCP_DEBUG == 1
std
::
cout
<<
"Tdcp iteration "
<<
j
<<
std
::
endl
;
std
::
cout
<<
"
\t
Excluded sats : "
;
std
::
cout
<<
"
\t
Common sats : "
<<
common_sats
.
size
()
<<
std
::
endl
;
std
::
cout
<<
"
\t
Rows : "
<<
n_differences
<<
std
::
endl
;
std
::
cout
<<
"
\t
Excluded rows : "
;
for
(
auto
excl_row
:
raim_discarded_rows
)
std
::
cout
<<
excl_row
<<
" "
;
std
::
cout
<<
std
::
endl
;
std
::
cout
<<
"
\t
Initial guess = "
<<
d_0
.
transpose
()
<<
" ("
<<
d_0
.
head
<
3
>
().
norm
()
<<
"m)"
<<
std
::
endl
;
std
::
cout
<<
"
\t
Solution = "
<<
d
.
transpose
()
<<
" ("
<<
d
.
head
<
3
>
().
norm
()
<<
"m)"
<<
std
::
endl
;
std
::
cout
<<
"
\t
Clock offset = "
<<
d
(
3
)
<<
std
::
endl
;
...
...
@@ -443,19 +455,17 @@ bool Tdcp(SnapshotPtr snapshot_r,
#endif
}
// weight covariance with the measurement noise (proportional to time)
double
sq_sigma_Tdcp
=
(
tk
-
tr
)
*
tdcp_params
.
tdcp
.
sigma_atm
*
tdcp_params
.
tdcp
.
sigma_atm
+
2
*
tdcp_params
.
tdcp
.
sigma_carrier
*
tdcp_params
.
tdcp
.
sigma_carrier
;
cov_d
*=
sq_sigma_Tdcp
;
// residual = (r - A * d).norm() / A.rows();
// Computing error on measurement space
for
(
int
row
=
0
;
row
<
n_differences
;
row
++
)
r
(
row
)
=
drho_m
(
row
)
-
(
s_k
.
col
(
row
)
-
x_r
-
d
.
head
(
3
)).
norm
()
+
(
s_r
.
col
(
row
)
-
x_r
).
norm
()
-
d
(
3
);
r
.
setZero
();
for
(
int
row
=
0
;
row
<
r
.
size
();
row
++
)
if
(
raim_discarded_rows
.
count
(
row
)
==
0
)
r
(
row
)
=
drho_m
(
row
)
-
(
s_k
.
col
(
row
)
-
x_r
-
d
.
head
(
3
)).
norm
()
+
(
s_r
.
col
(
row
)
-
x_r
).
norm
()
-
d
(
3
);
residual
=
sqrt
(
r
.
squaredNorm
()
/
r
.
size
());
// residual = (r - A * d).norm() / A.rows();
residual
=
sqrt
(
r
.
squaredNorm
()
/
(
r
.
size
()
-
raim_discarded_rows
.
size
()));
return
true
;
}
}
// namespace GnssUtils
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