//--------LICENSE_START-------- // // Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu) // All rights reserved. // // This file is part of gnss_utils // gnss_utils is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published by // the Free Software Foundation, either version 3 of the License, or // at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU Lesser General Public License for more details. // // You should have received a copy of the GNU Lesser General Public License // along with this program. If not, see <http://www.gnu.org/licenses/>. // //--------LICENSE_END-------- #include "gtest/utils_gtest.h" #include "gnss_utils/utils/satellite.h" #include "gnss_utils/utils/transformations.h" // 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; Eigen::Vector3d computeRandomReceiverLatLonAlt() { Eigen::Vector3d receiver_LLA = Eigen::Vector3d::Random(); // in [-1, 1] receiver_LLA(0) *= M_PI / 2; // in [-PI/2, PI/2] receiver_LLA(1) *= M_PI; // in [-PI, PI] receiver_LLA(2) += 1; // ([0, 2]) receiver_LLA(2) *= 5e2; // in [0, 1e3] return receiver_LLA; } void computeRandomVisibleSatellite(const Eigen::Vector3d& receiver_latlonalt, Eigen::Vector3d& sat_ENU, Eigen::Vector3d& sat_ECEF, Eigen::Vector2d& sat_azel, double range) { Eigen::Vector3d t_ECEF_ENU, t_ENU_ECEF; Eigen::Matrix3d R_ENU_ECEF; t_ECEF_ENU = latLonAltToEcef(receiver_latlonalt); computeEnuEcefFromLatLonAlt(receiver_latlonalt, R_ENU_ECEF, t_ENU_ECEF); // random elevation and azimuth sat_azel = Eigen::Vector2d::Random(); // in [-1, 1] sat_azel(0) *= M_PI; // in [-pi, pi] sat_azel(1) = (sat_azel(1)/2 + 0.5) * M_PI / 2; // in [0, pi/2] range = Eigen::VectorXd::Random(1)(0) * 5e2 + 1e3; // in [500, 1500] // ENU sat_ENU << sin(sat_azel(0)) * cos(sat_azel(1)) * range, cos(sat_azel(0)) * cos(sat_azel(1)) * range, sin(sat_azel(1)) * range; // ECEF sat_ECEF = t_ECEF_ENU + R_ENU_ECEF.transpose() * sat_ENU; } TEST(TransformationsTest, ecefToLatLonAlt) { Eigen::Vector3d ecef, latlonalt; // 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); 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 = computeRandomReceiverLatLonAlt(); 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 = computeRandomReceiverLatLonAlt(); 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, computeAzel) { Eigen::Vector3d t_ENU_ECEF,t_ECEF_ENU,t_ENU_latlonalt, sat_ENU, sat_ECEF; Eigen::Matrix3d R_ENU_ECEF; Eigen::Vector2d azel, azel2; double range; // random receiver position for (auto i = 0; i<100; i++) { t_ENU_latlonalt = computeRandomReceiverLatLonAlt(); t_ECEF_ENU = latLonAltToEcef(t_ENU_latlonalt); // random elevation and heading for (auto j = 0; j<100; j++) { computeRandomVisibleSatellite(t_ENU_latlonalt, sat_ENU, sat_ECEF, azel, range); azel2 = computeAzel(sat_ECEF, t_ECEF_ENU); ASSERT_MATRIX_APPROX(azel,azel2,1e-6); } } } int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); }