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
99352332
Commit
99352332
authored
5 years ago
by
Joan Vallvé Navarro
Browse files
Options
Downloads
Patches
Plain Diff
transformations and gtest
parent
a5b6ac84
No related branches found
No related tags found
2 merge requests
!20
new tag
,
!19
new tag
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
src/gnss_utils.cpp
+1
-1
1 addition, 1 deletion
src/gnss_utils.cpp
src/test/gtest_transformations.cpp
+180
-13
180 additions, 13 deletions
src/test/gtest_transformations.cpp
with
181 additions
and
14 deletions
src/gnss_utils.cpp
+
1
−
1
View file @
99352332
...
...
@@ -399,7 +399,7 @@ double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Ve
// receiver-sat vector enu (receiver ecef as origin)
Eigen
::
Vector3d
receiver_sat_enu
;
ecef2enu
(
receiver_geo
.
data
(),
//geodetic position {lat,lon} (rad)
receiver_sat_ecef
.
data
(),
//vector in ecef coordinate {x,y,z}
receiver_sat_ecef
.
data
(),
//vector in ecef coordinate {x,y,z}
receiver_sat_enu
.
data
());
// vector in local tangental coordinate {e,n,u}
// elevation
...
...
This diff is collapsed.
Click to expand it.
src/test/gtest_transformations.cpp
+
180
−
13
View file @
99352332
#include
"gtest/utils_gtest.h"
#include
"gnss_utils/gnss_utils.h"
/* functions being tested:
*
* Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d & _ecef);
* Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d & _latlon);
* Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_ecef);
* Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_enu);
* void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF);
* void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF);
*/
// Geodetic system parameters
static
double
kSemimajorAxis
=
6378137
;
static
double
kSemiminorAxis
=
6356752.3142
;
static
double
kFirstEccentricitySquared
=
6.69437999014
*
0.001
;
static
double
kSecondEccentricitySquared
=
6.73949674228
*
0.001
;
using
namespace
GNSSUtils
;
TEST
(
TransformationsTest
,
ecefToLatLonAlt
)
{
Eigen
::
Vector3d
ecef
,
latlonalt
;
ecef
<<
0
,
0
,
1e3
;
// aligned with ECEF X axis
ecef
<<
1e3
,
0
,
0
;
latlonalt
=
ecefToLatLonAlt
(
ecef
);
ASSERT_NEAR
(
latlonalt
(
0
),
0.0
,
1e-6
);
ASSERT_NEAR
(
latlonalt
(
1
),
0.0
,
1e-6
);
// aligned with ECEF Y axis
ecef
<<
0
,
1e3
,
0
;
latlonalt
=
ecefToLatLonAlt
(
ecef
);
ASSERT_NEAR
(
latlonalt
(
0
),
0.0
,
1e-6
);
ASSERT_NEAR
(
latlonalt
(
1
),
M_PI
/
2
,
1e-6
);
// aligned with ECEF Z axis
ecef
<<
0
,
0
,
1e3
;
latlonalt
=
ecefToLatLonAlt
(
ecef
);
std
::
cout
<<
latlonalt
.
transpose
();
ASSERT_NEAR
(
latlonalt
(
0
),
M_PI
/
2
,
1e-6
);
ASSERT_NEAR
(
latlonalt
(
1
),
0.0
,
1e-6
);
}
TEST
(
TransformationsTest
,
latLonAltToEcef
)
{
Eigen
::
Vector3d
ecef
,
latlonalt
;
// aligned with ECEF X axis
latlonalt
<<
0
,
0
,
0
;
ecef
=
latLonAltToEcef
(
latlonalt
);
ASSERT_NEAR
(
ecef
(
1
),
0.0
,
1e-6
);
ASSERT_NEAR
(
ecef
(
2
),
0.0
,
1e-6
);
// aligned with ECEF Y axis
latlonalt
<<
0
,
M_PI
/
2
,
0
;
ecef
=
latLonAltToEcef
(
latlonalt
);
ASSERT_NEAR
(
ecef
(
0
),
0.0
,
1e-6
);
ASSERT_NEAR
(
ecef
(
2
),
0.0
,
1e-6
);
// aligned with ECEF Z axis
latlonalt
<<
M_PI
/
2
,
0
,
0
;
ecef
=
latLonAltToEcef
(
latlonalt
);
ASSERT_NEAR
(
ecef
(
0
),
0.0
,
1e-6
);
ASSERT_NEAR
(
ecef
(
1
),
0.0
,
1e-6
);
}
TEST
(
TransformationsTest
,
latLonAltToEcef_ecefToLatLonAlt
)
{
Eigen
::
Vector3d
ecef0
,
ecef1
,
latlonalt0
,
latlonalt1
;
// ecef -> latlon -> ecef
for
(
auto
i
=
0
;
i
<
100
;
i
++
)
{
ecef0
=
1e3
*
Eigen
::
Vector3d
::
Random
();
latlonalt0
=
ecefToLatLonAlt
(
ecef0
);
ecef1
=
latLonAltToEcef
(
latlonalt0
);
ASSERT_MATRIX_APPROX
(
ecef0
,
ecef1
,
1e-6
);
}
// latlon -> ecef -> latlon
for
(
auto
i
=
0
;
i
<
100
;
i
++
)
{
latlonalt0
=
Eigen
::
Vector3d
::
Random
();
latlonalt0
(
0
)
*=
M_PI
/
2
;
latlonalt0
(
1
)
*=
M_PI
;
latlonalt0
(
2
)
*=
1e3
;
ecef0
=
latLonAltToEcef
(
latlonalt0
);
latlonalt1
=
ecefToLatLonAlt
(
ecef0
);
ASSERT_MATRIX_APPROX
(
latlonalt0
,
latlonalt1
,
1e-6
);
}
}
TEST
(
TransformationsTest
,
computeEnuEcef
)
{
Eigen
::
Vector3d
t_ECEF_ENU
,
t_ENU_latlonalt
,
latlonalt1
;
Eigen
::
Vector3d
t_ENU_ECEF1
,
t_ENU_ECEF2
,
t_ENU_ECEF3
;
Eigen
::
Matrix3d
R_ENU_ECEF1
,
R_ENU_ECEF2
,
R_ENU_ECEF3
;
// random
for
(
auto
i
=
0
;
i
<
100
;
i
++
)
{
t_ENU_latlonalt
=
Eigen
::
Vector3d
::
Random
();
t_ENU_latlonalt
(
0
)
*=
M_PI
/
2
;
t_ENU_latlonalt
(
1
)
*=
M_PI
;
t_ENU_latlonalt
(
2
)
*=
1e3
;
t_ECEF_ENU
=
latLonAltToEcef
(
t_ENU_latlonalt
);
// 1 from ECEF
computeEnuEcefFromEcef
(
t_ECEF_ENU
,
R_ENU_ECEF1
,
t_ENU_ECEF1
);
// 2 from latlonalt
computeEnuEcefFromLatLonAlt
(
t_ENU_latlonalt
,
R_ENU_ECEF2
,
t_ENU_ECEF2
);
// 3 Hardcoded solution
/* Convert ECEF coordinates to geodetic coordinates.
* J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
* to geodetic coordinates," IEEE Transactions on Aerospace and
* Electronic Systems, vol. 30, pp. 957-961, 1994.
*/
double
r
=
std
::
sqrt
(
t_ECEF_ENU
(
0
)
*
t_ECEF_ENU
(
0
)
+
t_ECEF_ENU
(
1
)
*
t_ECEF_ENU
(
1
));
double
Esq
=
kSemimajorAxis
*
kSemimajorAxis
-
kSemiminorAxis
*
kSemiminorAxis
;
double
F
=
54
*
kSemiminorAxis
*
kSemiminorAxis
*
t_ECEF_ENU
(
2
)
*
t_ECEF_ENU
(
2
);
double
G
=
r
*
r
+
(
1
-
kFirstEccentricitySquared
)
*
t_ECEF_ENU
(
2
)
*
t_ECEF_ENU
(
2
)
-
kFirstEccentricitySquared
*
Esq
;
double
C
=
(
kFirstEccentricitySquared
*
kFirstEccentricitySquared
*
F
*
r
*
r
)
/
pow
(
G
,
3
);
double
S
=
cbrt
(
1
+
C
+
sqrt
(
C
*
C
+
2
*
C
));
double
P
=
F
/
(
3
*
pow
((
S
+
1
/
S
+
1
),
2
)
*
G
*
G
);
double
Q
=
sqrt
(
1
+
2
*
kFirstEccentricitySquared
*
kFirstEccentricitySquared
*
P
);
double
r_0
=
-
(
P
*
kFirstEccentricitySquared
*
r
)
/
(
1
+
Q
)
+
sqrt
(
0.5
*
kSemimajorAxis
*
kSemimajorAxis
*
(
1
+
1.0
/
Q
)
-
P
*
(
1
-
kFirstEccentricitySquared
)
*
t_ECEF_ENU
(
2
)
*
t_ECEF_ENU
(
2
)
/
(
Q
*
(
1
+
Q
))
-
0.5
*
P
*
r
*
r
);
double
V
=
sqrt
(
pow
((
r
-
kFirstEccentricitySquared
*
r_0
),
2
)
+
(
1
-
kFirstEccentricitySquared
)
*
t_ECEF_ENU
(
2
)
*
t_ECEF_ENU
(
2
));
double
Z_0
=
kSemiminorAxis
*
kSemiminorAxis
*
t_ECEF_ENU
(
2
)
/
(
kSemimajorAxis
*
V
);
double
latitude
=
atan
((
t_ECEF_ENU
(
2
)
+
kSecondEccentricitySquared
*
Z_0
)
/
r
);
double
longitude
=
atan2
(
t_ECEF_ENU
(
1
),
t_ECEF_ENU
(
0
));
double
sLat
=
sin
(
latitude
);
double
cLat
=
cos
(
latitude
);
double
sLon
=
sin
(
longitude
);
double
cLon
=
cos
(
longitude
);
R_ENU_ECEF3
(
0
,
0
)
=
-
sLon
;
R_ENU_ECEF3
(
0
,
1
)
=
cLon
;
R_ENU_ECEF3
(
0
,
2
)
=
0.0
;
R_ENU_ECEF3
(
1
,
0
)
=
-
sLat
*
cLon
;
R_ENU_ECEF3
(
1
,
1
)
=
-
sLat
*
sLon
;
R_ENU_ECEF3
(
1
,
2
)
=
cLat
;
R_ENU_ECEF3
(
2
,
0
)
=
cLat
*
cLon
;
R_ENU_ECEF3
(
2
,
1
)
=
cLat
*
sLon
;
R_ENU_ECEF3
(
2
,
2
)
=
sLat
;
t_ENU_ECEF3
=
-
R_ENU_ECEF3
*
t_ECEF_ENU
;
// CHECK
ASSERT_MATRIX_APPROX
(
t_ENU_ECEF1
,
t_ENU_ECEF2
,
1e-6
);
ASSERT_MATRIX_APPROX
(
R_ENU_ECEF1
,
R_ENU_ECEF2
,
1e-6
);
ASSERT_MATRIX_APPROX
(
t_ENU_ECEF1
,
t_ENU_ECEF3
,
1e-6
);
ASSERT_MATRIX_APPROX
(
R_ENU_ECEF1
,
R_ENU_ECEF3
,
1e-6
);
}
}
TEST
(
TransformationsTest
,
computeSatElevation
)
{
Eigen
::
Vector3d
t_ENU_ECEF
,
t_ECEF_ENU
,
t_ENU_latlonalt
,
rec_sat_ENU
,
sat_ECEF
;
Eigen
::
Matrix3d
R_ENU_ECEF
;
// random receiver position
for
(
auto
i
=
0
;
i
<
100
;
i
++
)
{
t_ENU_latlonalt
=
Eigen
::
Vector3d
::
Random
();
t_ENU_latlonalt
(
0
)
*=
M_PI
/
2
;
t_ENU_latlonalt
(
1
)
*=
M_PI
;
t_ENU_latlonalt
(
2
)
*=
1e3
;
t_ECEF_ENU
=
latLonAltToEcef
(
t_ENU_latlonalt
);
computeEnuEcefFromLatLonAlt
(
t_ENU_latlonalt
,
R_ENU_ECEF
,
t_ENU_ECEF
);
// random elevation and heading
for
(
auto
j
=
0
;
j
<
100
;
j
++
)
{
Eigen
::
Vector2d
rand2
=
Eigen
::
Vector2d
::
Random
();
double
heading
=
rand2
(
0
)
*
M_PI
;
double
elevation
=
rand2
(
0
)
*
M_PI
/
2
;
rec_sat_ENU
<<
cos
(
heading
)
*
1000
,
sin
(
heading
)
*
1000
,
tan
(
elevation
)
*
1000
;
sat_ECEF
=
t_ECEF_ENU
+
R_ENU_ECEF
.
transpose
()
*
rec_sat_ENU
;
double
elevation2
=
computeSatElevation
(
t_ECEF_ENU
,
sat_ECEF
);
ASSERT_DOUBLE_EQ
(
latlonalt
(
0
),
0.0
);
ASSERT_DOUBLE_EQ
(
latlonalt
(
1
),
0.0
);
ASSERT_NEAR
(
elevation
,
elevation2
,
1e-6
);
}
}
}
int
main
(
int
argc
,
char
**
argv
)
...
...
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