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

transformations and gtest

parent a5b6ac84
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
......@@ -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
......
#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)
......
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