From 5d5eb5bd455d62c58bd32a0a78b3b656b29cca3c Mon Sep 17 00:00:00 2001 From: Oliver Lemke Date: Tue, 12 May 2026 12:08:32 +0200 Subject: [PATCH 1/6] refactor: use non-iterative Bowring algorithm for ecef2geodetic Replace the iterative ECEF-to-geodetic conversion with Bowring's closed-form algorithm. This removes special-casing for spherical ellipsoids and near-poles, and eliminates the convergence loop and its associated iteration limit. --- src/core/geodesy/geodetic.cpp | 56 +++++++++++++---------------------- src/core/geodesy/geodetic.h | 6 ++-- 2 files changed, 24 insertions(+), 38 deletions(-) diff --git a/src/core/geodesy/geodetic.cpp b/src/core/geodesy/geodetic.cpp index b4fc88822f..5bb0dbc9ba 100644 --- a/src/core/geodesy/geodetic.cpp +++ b/src/core/geodesy/geodetic.cpp @@ -128,46 +128,30 @@ std::pair ecef2geocentric_los(Vector3 ecef, Vector3 decef) { Vector3 ecef2geodetic(Vector3 ecef, Vector2 refellipsoid) { Vector3 pos; + // Bowring's closed-form algorithm (non-iterative) + const Numeric a = refellipsoid[0]; + const Numeric b = refellipsoid[1]; + const Numeric e2 = 1.0 - (b * b) / (a * a); + const Numeric ep2 = (a * a) / (b * b) - 1.0; // 2nd eccentricity squared - // Use geocentric function if geoid is spherical - if (is_ellipsoid_spherical(refellipsoid)) { - pos = ecef2geocentric(ecef); - pos[0] -= refellipsoid[0]; + const Numeric p = std::hypot(ecef[0], ecef[1]); + const Numeric z = ecef[2]; - } else { - // The general algorithm not stable for lat=+-90. Catch these cases - // Also catch near-pole cases where numerical instability can occur - const Numeric sq = std::hypot(ecef[0], ecef[1]); + const Numeric theta = atan2(z * a, p * b); + const Numeric st = sin(theta); + const Numeric ct = cos(theta); + + const Numeric latrad = + atan2(z + ep2 * b * st * st * st, p - e2 * a * ct * ct * ct); + const Numeric sinlat = sin(latrad); + const Numeric coslat = cos(latrad); - if (sq < near_pole_threshold_ecef * refellipsoid[0]) { - // Near-pole case: use simplified formula (same as exact pole) - pos[0] = fabs(ecef[2]) - refellipsoid[1]; - pos[1] = ecef[2] >= 0 ? 90 : -90; - pos[2] = RAD2DEG * atan2(ecef[1], ecef[0]); + const Numeric N = a / sqrt(1.0 - e2 * sinlat * sinlat); - } else { - // General algorithm - pos[2] = RAD2DEG * atan2(ecef[1], ecef[0]); - - Numeric B0 = atan2(ecef[2], sq); - Numeric B = B0 - 1, N; - const Numeric e2 = 1 - (refellipsoid[1] * refellipsoid[1]) / - (refellipsoid[0] * refellipsoid[0]); - Index num_iter = 0; - // 1e-15 seems to give a accuracy of better than 2 cm - while (fabs(B - B0) > 1e-15 && num_iter < max_ecef2geodetic_iter) { - N = refellipsoid[0] / sqrt(1 - e2 * sin(B0) * sin(B0)); - pos[0] = sq / cos(B0) - N; - B = B0; - B0 = atan((ecef[2] / sq) * 1 / (1 - e2 * N / (N + pos[0]))); - ++num_iter; - } - ARTS_USER_ERROR_IF(num_iter == max_ecef2geodetic_iter, - "ECEF to geodetic conversion did not converge. " - "Input may be too close to singular values."); - pos[1] = RAD2DEG * B; - } - } + // Robust height formula avoiding division by cos(lat) + pos[0] = p * coslat + z * sinlat - a * a / N; + pos[1] = RAD2DEG * latrad; + pos[2] = RAD2DEG * atan2(ecef[1], ecef[0]); return pos; } diff --git a/src/core/geodesy/geodetic.h b/src/core/geodesy/geodetic.h index 26f98433b2..7a26503f08 100644 --- a/src/core/geodesy/geodetic.h +++ b/src/core/geodesy/geodetic.h @@ -43,12 +43,14 @@ std::pair ecef2geocentric_los(Vector3 ecef, Vector3 decef); /** Conversion from ECEF to geodetic coordinates + Uses a non-iterative, closed-form (Bowring-style) algorithm. + @param[in] ecef ECEF position (x,y,z) @param[in] refellipsoid As the WSV with same name. @return pos Geodetic position (h,lat,lon) - @author Patrick Eriksson - @date 2021-07-29 + @author Oliver Lemke + @date 2026-05-12 */ Vector3 ecef2geodetic(Vector3 ecef, Vector2 refellipsoid); From 0ecc26ce733a39639adab80b375d35573e79c90c Mon Sep 17 00:00:00 2001 From: Oliver Lemke Date: Wed, 20 May 2026 11:28:13 +0200 Subject: [PATCH 2/6] Remove unused constants from geodetic.cpp --- src/core/geodesy/geodetic.cpp | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/src/core/geodesy/geodetic.cpp b/src/core/geodesy/geodetic.cpp index 5bb0dbc9ba..b270220cf6 100644 --- a/src/core/geodesy/geodetic.cpp +++ b/src/core/geodesy/geodetic.cpp @@ -47,21 +47,6 @@ inline constexpr Numeric ellipsoid_radii_threshold = 1e-3; inline constexpr Numeric POLELATZZZ = 90 - 1e-8; // Rename to POLELAT when other one removed -/** Threshold for near-pole detection in ECEF to geodetic conversion - - When the horizontal distance from the z-axis (sqrt(x^2 + y^2)) is - smaller than this threshold times the semi-major axis, the point - is treated as being on the pole to avoid numerical instability. -*/ -inline constexpr Numeric near_pole_threshold_ecef = 1e-15; - -/** Maximum iterations for ECEF to geodetic conversion - - Prevents infinite loops in edge cases where the iterative - algorithm fails to converge. -*/ -inline constexpr Index max_ecef2geodetic_iter = 100; - /*=========================================================================== === The functions, in alphabetical order ===========================================================================*/ From f34d6632d0346bf0324987bf2fbc9aaaef65e2f0 Mon Sep 17 00:00:00 2001 From: Oliver Lemke Date: Wed, 20 May 2026 11:29:12 +0200 Subject: [PATCH 3/6] test: add geodetic conversion accuracy and performance tests --- src/core/tests/CMakeLists.txt | 5 + src/core/tests/test_geodetic.cpp | 272 +++++++++++++++++++++++++++++++ 2 files changed, 277 insertions(+) create mode 100644 src/core/tests/test_geodetic.cpp diff --git a/src/core/tests/CMakeLists.txt b/src/core/tests/CMakeLists.txt index f18def6089..0f803f22b4 100644 --- a/src/core/tests/CMakeLists.txt +++ b/src/core/tests/CMakeLists.txt @@ -15,3 +15,8 @@ add_executable(test_laginterp test_laginterp.cpp) target_link_libraries(test_laginterp PUBLIC matpack rng) add_test(NAME "cpp.fast.core.test_laginterp" COMMAND test_laginterp) add_dependencies(check-deps test_laginterp) + +add_executable(test_geodetic test_geodetic.cpp) +target_link_libraries(test_geodetic PUBLIC geodesy matpack) +add_test(NAME "cpp.fast.core.test_geodetic" COMMAND test_geodetic) +add_dependencies(check-deps test_geodetic) diff --git a/src/core/tests/test_geodetic.cpp b/src/core/tests/test_geodetic.cpp new file mode 100644 index 0000000000..da702e5e28 --- /dev/null +++ b/src/core/tests/test_geodetic.cpp @@ -0,0 +1,272 @@ +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "time_test_util.h" + +namespace { +struct Stats { + Numeric max_dh = 0; + Numeric max_dlat = 0; + Numeric max_dlon = 0; + Numeric avg_dh = 0; + Numeric avg_dlat = 0; + Numeric avg_dlon = 0; + Index count = 0; +}; + +void update_stats(Stats& s, const Vector3& old_pos, const Vector3& new_pos) { + const Numeric dh = std::abs(old_pos[0] - new_pos[0]); + const Numeric dlat = std::abs(old_pos[1] - new_pos[1]); + const Numeric dlon = std::abs(old_pos[2] - new_pos[2]); + + s.max_dh = std::max(s.max_dh, dh); + s.max_dlat = std::max(s.max_dlat, dlat); + s.max_dlon = std::max(s.max_dlon, dlon); + + s.avg_dh += dh; + s.avg_dlat += dlat; + s.avg_dlon += dlon; + ++s.count; +} + +void print_stats(const Stats& s, std::string_view label) { + if (s.count == 0) { + std::println("{}: no samples", label); + return; + } + const auto n = static_cast(s.count); + std::println(R"({}: + max_dh = {:.6e} m + max_dlat = {:.6e} deg + max_dlon = {:.6e} deg + avg_dh = {:.6e} m + avg_dlat = {:.6e} deg + avg_dlon = {:.6e} deg + samples = {})", + label, + s.max_dh, + s.max_dlat, + s.max_dlon, + s.avg_dh / n, + s.avg_dlat / n, + s.avg_dlon / n, + s.count); +} + +inline constexpr Numeric RAD2DEG = Conversion::rad2deg(1); + +/** Threshold for near-pole detection in ECEF to geodetic conversion + + When the horizontal distance from the z-axis (sqrt(x^2 + y^2)) is + smaller than this threshold times the semi-major axis, the point + is treated as being on the pole to avoid numerical instability. +*/ +inline constexpr Numeric near_pole_threshold_ecef = 1e-15; + +/** Maximum iterations for ECEF to geodetic conversion + + Prevents infinite loops in edge cases where the iterative + algorithm fails to converge. +*/ +inline constexpr Index max_ecef2geodetic_iter = 100; + +/** Original iterative algorithm for ECEF to geodetic conversion */ +Vector3 ecef2geodetic_old(Vector3 ecef, Vector2 refellipsoid) { + Vector3 pos; + + // Use geocentric function if geoid is spherical + if (is_ellipsoid_spherical(refellipsoid)) { + pos = ecef2geocentric(ecef); + pos[0] -= refellipsoid[0]; + + } else { + // The general algorithm not stable for lat=+-90. Catch these cases + // Also catch near-pole cases where numerical instability can occur + const Numeric sq = std::hypot(ecef[0], ecef[1]); + + if (sq < near_pole_threshold_ecef * refellipsoid[0]) { + // Near-pole case: use simplified formula (same as exact pole) + pos[0] = fabs(ecef[2]) - refellipsoid[1]; + pos[1] = ecef[2] >= 0 ? 90 : -90; + pos[2] = RAD2DEG * atan2(ecef[1], ecef[0]); + + } else { + // General algorithm + pos[2] = RAD2DEG * atan2(ecef[1], ecef[0]); + + Numeric B0 = atan2(ecef[2], sq); + Numeric B = B0 - 1, N; + const Numeric e2 = 1 - (refellipsoid[1] * refellipsoid[1]) / + (refellipsoid[0] * refellipsoid[0]); + Index num_iter = 0; + // 1e-15 seems to give a accuracy of better than 2 cm + while (fabs(B - B0) > 1e-15 && num_iter < max_ecef2geodetic_iter) { + N = refellipsoid[0] / sqrt(1 - e2 * sin(B0) * sin(B0)); + pos[0] = sq / cos(B0) - N; + B = B0; + B0 = atan((ecef[2] / sq) * 1 / (1 - e2 * N / (N + pos[0]))); + ++num_iter; + } + ARTS_USER_ERROR_IF(num_iter == max_ecef2geodetic_iter, + "ECEF to geodetic conversion did not converge. " + "Input may be too close to singular values."); + pos[1] = RAD2DEG * B; + } + } + + return pos; +} + +} // namespace + +int main() { + // WGS-84 ellipsoid (a, b) + const Vector2 wgs84{Body::Earth::a, Body::Earth::b}; + const Vector2 sphere{6371000.0, 6371000.0}; + + // Test grids + const Vector alts{ + -1000.0, + 0.0, + 100.0, + 1000.0, + 10000.0, + 100000.0, + 250000.0, + 3.6e8, // Distance to the Moon (m) + 3.3e11, // Distance to Mars (m) + 8.7e11, // Distance to Jupiter (m) + 1.5e12, // Distance to Saturn (m) + 3.1e12, // Distance to Uranus (m) + 5.2e12, // Distance to Pluto (m) + }; + const Vector lats{-90.0, + -89.99, + -80.0, + -60.0, + -45.0, + -30.0, + -15.0, + 0.0, + 15.0, + 30.0, + 45.0, + 60.0, + 80.0, + 89.99, + 90.0}; + const Vector lons{ + -180.0, -135.0, -90.0, -45.0, 0.0, 45.0, 90.0, 135.0, 180.0}; + + Stats stats_wgs84_high; // WGS-84 overall for high altitude (>100km) + Stats stats_wgs84_low; // WGS-84 overall for low altitude (<=100km) + Stats stats_sphere_high; // Sphere for high altitude (>100km) + Stats stats_sphere_low; // Sphere for low altitude (<=100km) + Stats stats_poles_high; // |lat| > 89.9 for high altitude (>100km) + Stats stats_poles_low; // |lat| > 89.9 for low altitude (<=100km) + Stats stats_mid_high; // |lat| < 80 for high altitude (>100km) + Stats stats_mid_low; // |lat| < 80 for low altitude (<=100km) + + // --- Accuracy test --- + for (Numeric h : alts) { + for (Numeric lat : lats) { + for (Numeric lon : lons) { + Vector3 geo{h, lat, lon}; + + const bool is_high_altitude = std::abs(h) > 100e3; + + // Round-trip via old function + Vector3 ecef = geodetic2ecef(geo, wgs84); + Vector3 old_pos = ecef2geodetic_old(ecef, wgs84); + Vector3 new_pos = ecef2geodetic(ecef, wgs84); + + if (is_high_altitude) + update_stats(stats_wgs84_high, old_pos, new_pos); + else + update_stats(stats_wgs84_low, old_pos, new_pos); + + if (std::abs(lat) > 89.9) { + if (is_high_altitude) + update_stats(stats_poles_high, old_pos, new_pos); + else + update_stats(stats_poles_low, old_pos, new_pos); + } else if (std::abs(lat) < 80.0) { + if (is_high_altitude) + update_stats(stats_mid_high, old_pos, new_pos); + else + update_stats(stats_mid_low, old_pos, new_pos); + } + + // Spherical case + Vector3 ecef_s = geodetic2ecef(geo, sphere); + Vector3 old_s = ecef2geodetic_old(ecef_s, sphere); + Vector3 new_s = ecef2geodetic(ecef_s, sphere); + if (is_high_altitude) + update_stats(stats_sphere_high, old_s, new_s); + else + update_stats(stats_sphere_low, old_s, new_s); + } + } + } + + std::println("=== Accuracy comparison (old vs new) ==="); + print_stats(stats_wgs84_low, "WGS-84 overall (low altitude <=100km)"); + print_stats(stats_wgs84_high, "WGS-84 overall (high altitude >100km)"); + print_stats(stats_poles_low, "Near-pole (low altitude <=100km)"); + print_stats(stats_poles_high, "Near-pole (high altitude >100km)"); + print_stats(stats_mid_low, "Mid-latitude (low altitude <=100km)"); + print_stats(stats_mid_high, "Mid-latitude (high altitude >100km)"); + print_stats(stats_sphere_low, "Sphere (low altitude <=100km)"); + print_stats(stats_sphere_high, "Sphere (high altitude >100km)"); + + // --- Performance test --- + constexpr Index n_perf = 1'000'000; + + // Build a list of ECEF points for benchmarking + std::vector ecef_points; + ecef_points.reserve(lats.size() * lons.size() * alts.size()); + for (Numeric h : alts) { + for (Numeric lat : lats) { + for (Numeric lon : lons) { + ecef_points.push_back(geodetic2ecef(Vector3{h, lat, lon}, wgs84)); + } + } + } + + // Warm-up to reduce cache effects + for (const auto& e : ecef_points) { + volatile Vector3 p1 = ecef2geodetic_old(e, wgs84); + volatile Vector3 p2 = ecef2geodetic(e, wgs84); + (void)p1; + (void)p2; + } + + { + test_timer_t timer("ecef2geodetic (old)"); + for (Index i = 0; i < n_perf; ++i) { + volatile Vector3 p = + ecef2geodetic_old(ecef_points[i % ecef_points.size()], wgs84); + (void)p; + } + } + + { + test_timer_t timer("ecef2geodetic_new"); + for (Index i = 0; i < n_perf; ++i) { + volatile Vector3 p = + ecef2geodetic(ecef_points[i % ecef_points.size()], wgs84); + (void)p; + } + } + + print_time_points(); + + return 0; +} From 32795e9ee71c197c4e4d3055d06d8dfbd3e4b0de Mon Sep 17 00:00:00 2001 From: Oliver Lemke Date: Wed, 20 May 2026 11:50:40 +0200 Subject: [PATCH 4/6] refactor: improve numerical stability in ecef2geodetic MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replace direct computation of e2 and ep2 with algebraically equivalent expressions that avoid catastrophic cancellation. Using (a-b)(a+b) = a² - b² identity prevents precision loss when a and b are close in value (as is the case for Earth reference ellipsoids where flattening is small). - Compute a_minus_b = a - b and a_plus_b = a + b - Express e2 and ep2 using these intermediates --- src/core/geodesy/geodetic.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/core/geodesy/geodetic.cpp b/src/core/geodesy/geodetic.cpp index b270220cf6..7fab9c198d 100644 --- a/src/core/geodesy/geodetic.cpp +++ b/src/core/geodesy/geodetic.cpp @@ -114,10 +114,12 @@ std::pair ecef2geocentric_los(Vector3 ecef, Vector3 decef) { Vector3 ecef2geodetic(Vector3 ecef, Vector2 refellipsoid) { Vector3 pos; // Bowring's closed-form algorithm (non-iterative) - const Numeric a = refellipsoid[0]; - const Numeric b = refellipsoid[1]; - const Numeric e2 = 1.0 - (b * b) / (a * a); - const Numeric ep2 = (a * a) / (b * b) - 1.0; // 2nd eccentricity squared + const Numeric a = refellipsoid[0]; + const Numeric b = refellipsoid[1]; + const Numeric a_minus_b = a - b; + const Numeric a_plus_b = a + b; + const Numeric e2 = (a_minus_b * a_plus_b) / (a * a); + const Numeric ep2 = (a_minus_b * a_plus_b) / (b * b); const Numeric p = std::hypot(ecef[0], ecef[1]); const Numeric z = ecef[2]; From fb147e18589916caa440abf665bb1a9952a27f0d Mon Sep 17 00:00:00 2001 From: Oliver Lemke Date: Thu, 21 May 2026 07:39:21 +0200 Subject: [PATCH 5/6] test: add round-trip error stats for geodetic conversions --- src/core/tests/test_geodetic.cpp | 60 +++++++++++++++++++++++++++----- 1 file changed, 51 insertions(+), 9 deletions(-) diff --git a/src/core/tests/test_geodetic.cpp b/src/core/tests/test_geodetic.cpp index da702e5e28..d3c4516f8a 100644 --- a/src/core/tests/test_geodetic.cpp +++ b/src/core/tests/test_geodetic.cpp @@ -165,14 +165,22 @@ int main() { const Vector lons{ -180.0, -135.0, -90.0, -45.0, 0.0, 45.0, 90.0, 135.0, 180.0}; - Stats stats_wgs84_high; // WGS-84 overall for high altitude (>100km) - Stats stats_wgs84_low; // WGS-84 overall for low altitude (<=100km) - Stats stats_sphere_high; // Sphere for high altitude (>100km) - Stats stats_sphere_low; // Sphere for low altitude (<=100km) - Stats stats_poles_high; // |lat| > 89.9 for high altitude (>100km) - Stats stats_poles_low; // |lat| > 89.9 for low altitude (<=100km) - Stats stats_mid_high; // |lat| < 80 for high altitude (>100km) - Stats stats_mid_low; // |lat| < 80 for low altitude (<=100km) + Stats stats_wgs84_high; // WGS-84 overall for high altitude (>100km) + Stats stats_wgs84_low; // WGS-84 overall for low altitude (<=100km) + Stats stats_sphere_high; // Sphere for high altitude (>100km) + Stats stats_sphere_low; // Sphere for low altitude (<=100km) + Stats stats_poles_high; // |lat| > 89.9 for high altitude (>100km) + Stats stats_poles_low; // |lat| > 89.9 for low altitude (<=100km) + Stats stats_mid_high; // |lat| < 80 for high altitude (>100km) + Stats stats_mid_low; // |lat| < 80 for low altitude (<=100km) + Stats stats_wgs84_rt_low; // WGS-84 round-trip for low altitude (<=100km) + Stats stats_wgs84_rt_high; // WGS-84 round-trip for high altitude (>100km) + Stats stats_poles_rt_low; // Near-pole round-trip for low altitude (<=100km) + Stats stats_poles_rt_high; // Near-pole round-trip for high altitude (>100km) + Stats stats_mid_rt_low; // Mid-lat round-trip for low altitude (<=100km) + Stats stats_mid_rt_high; // Mid-lat round-trip for high altitude (>100km) + Stats stats_sphere_rt_low; // Sphere round-trip for low altitude (<=100km) + Stats stats_sphere_rt_high; // Sphere round-trip for high altitude (>100km) // --- Accuracy test --- for (Numeric h : alts) { @@ -192,16 +200,31 @@ int main() { else update_stats(stats_wgs84_low, old_pos, new_pos); + if (is_high_altitude) + update_stats(stats_wgs84_rt_high, geo, new_pos); + else + update_stats(stats_wgs84_rt_low, geo, new_pos); + if (std::abs(lat) > 89.9) { if (is_high_altitude) update_stats(stats_poles_high, old_pos, new_pos); else update_stats(stats_poles_low, old_pos, new_pos); + + if (is_high_altitude) + update_stats(stats_poles_rt_high, geo, new_pos); + else + update_stats(stats_poles_rt_low, geo, new_pos); } else if (std::abs(lat) < 80.0) { if (is_high_altitude) update_stats(stats_mid_high, old_pos, new_pos); else update_stats(stats_mid_low, old_pos, new_pos); + + if (is_high_altitude) + update_stats(stats_mid_rt_high, geo, new_pos); + else + update_stats(stats_mid_rt_low, geo, new_pos); } // Spherical case @@ -212,11 +235,16 @@ int main() { update_stats(stats_sphere_high, old_s, new_s); else update_stats(stats_sphere_low, old_s, new_s); + + if (is_high_altitude) + update_stats(stats_sphere_rt_high, geo, new_s); + else + update_stats(stats_sphere_rt_low, geo, new_s); } } } - std::println("=== Accuracy comparison (old vs new) ==="); + std::println("=== Accuracy comparison (old vs new ecef2geodetic) ==="); print_stats(stats_wgs84_low, "WGS-84 overall (low altitude <=100km)"); print_stats(stats_wgs84_high, "WGS-84 overall (high altitude >100km)"); print_stats(stats_poles_low, "Near-pole (low altitude <=100km)"); @@ -226,6 +254,20 @@ int main() { print_stats(stats_sphere_low, "Sphere (low altitude <=100km)"); print_stats(stats_sphere_high, "Sphere (high altitude >100km)"); + std::println("=== Round-trip error (original geodetic vs ecef2geodetic) ==="); + print_stats(stats_wgs84_rt_low, "WGS-84 round-trip (low altitude <=100km)"); + print_stats(stats_wgs84_rt_high, "WGS-84 round-trip (high altitude >100km)"); + print_stats(stats_poles_rt_low, + "Near-pole round-trip (low altitude <=100km)"); + print_stats(stats_poles_rt_high, + "Near-pole round-trip (high altitude >100km)"); + print_stats(stats_mid_rt_low, + "Mid-latitude round-trip (low altitude <=100km)"); + print_stats(stats_mid_rt_high, + "Mid-latitude round-trip (high altitude >100km)"); + print_stats(stats_sphere_rt_low, "Sphere round-trip (low altitude <=100km)"); + print_stats(stats_sphere_rt_high, "Sphere round-trip (high altitude >100km)"); + // --- Performance test --- constexpr Index n_perf = 1'000'000; From 76567e5a0e0490310532e9321732099618c427e4 Mon Sep 17 00:00:00 2001 From: Oliver Lemke Date: Thu, 21 May 2026 10:17:31 +0200 Subject: [PATCH 6/6] test: add LOS accuracy and round-trip tests for geodetic conversions Add comprehensive LOS tests to test_geodetic.cpp: - Compare ecef2geodetic_los internally using old/new ecef2geodetic implementations - Measure round-trip errors for geodetic_los2ecef -> ecef2geodetic_los --- src/core/tests/test_geodetic.cpp | 208 ++++++++++++++++++++++++++++++- 1 file changed, 207 insertions(+), 1 deletion(-) diff --git a/src/core/tests/test_geodetic.cpp b/src/core/tests/test_geodetic.cpp index d3c4516f8a..2afffd77fd 100644 --- a/src/core/tests/test_geodetic.cpp +++ b/src/core/tests/test_geodetic.cpp @@ -60,7 +60,52 @@ void print_stats(const Stats& s, std::string_view label) { s.count); } -inline constexpr Numeric RAD2DEG = Conversion::rad2deg(1); +struct LosStats { + Numeric max_dza = 0; + Numeric max_daa = 0; + Numeric avg_dza = 0; + Numeric avg_daa = 0; + Index count = 0; +}; + +void update_los_stats(LosStats& s, + const Vector2& old_los, + const Vector2& new_los) { + const Numeric dza = std::abs(old_los[0] - new_los[0]); + Numeric daa = std::abs(old_los[1] - new_los[1]); + if (daa > 180.0) daa = 360.0 - daa; + + s.max_dza = std::max(s.max_dza, dza); + s.max_daa = std::max(s.max_daa, daa); + + s.avg_dza += dza; + s.avg_daa += daa; + ++s.count; +} + +void print_los_stats(const LosStats& s, std::string_view label) { + if (s.count == 0) { + std::println("{}: no samples", label); + return; + } + const auto n = static_cast(s.count); + std::println(R"({}: + max_dza = {:.6e} deg + max_daa = {:.6e} deg + avg_dza = {:.6e} deg + avg_daa = {:.6e} deg + samples = {})", + label, + s.max_dza, + s.max_daa, + s.avg_dza / n, + s.avg_daa / n, + s.count); +} + +inline constexpr Numeric POLELATZZZ = 90 - 1e-8; +inline constexpr Numeric DEG2RAD = Conversion::deg2rad(1); +inline constexpr Numeric RAD2DEG = Conversion::rad2deg(1); /** Threshold for near-pole detection in ECEF to geodetic conversion @@ -124,6 +169,34 @@ Vector3 ecef2geodetic_old(Vector3 ecef, Vector2 refellipsoid) { return pos; } +std::pair ecef2geodetic_los_with_ecef2geodetic_old( + Vector3 ecef, Vector3 decef, Vector2 refellipsoid) { + Vector3 pos = ecef2geodetic_old(ecef, refellipsoid); + + const Numeric latrad = DEG2RAD * pos[1]; + const Numeric lonrad = DEG2RAD * pos[2]; + const Numeric coslat = cos(latrad); + const Numeric sinlat = sin(latrad); + const Numeric coslon = cos(lonrad); + const Numeric sinlon = sin(lonrad); + + // See + // https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ECEF_to_ENU + Vector3 enu; + enu[0] = -sinlon * decef[0] + coslon * decef[1]; + enu[1] = -sinlat * coslon * decef[0] - sinlat * sinlon * decef[1] + + coslat * decef[2]; + enu[2] = coslat * coslon * decef[0] + coslat * sinlon * decef[1] + + sinlat * decef[2]; + + Vector2 los = enu2los(enu); + + // Azimuth at poles needs special treatment + if (fabs(pos[1]) > POLELATZZZ) los[1] = RAD2DEG * atan2(decef[1], decef[0]); + + return {pos, los}; +} + } // namespace int main() { @@ -149,6 +222,7 @@ int main() { }; const Vector lats{-90.0, -89.99, + -89.91, -80.0, -60.0, -45.0, @@ -160,10 +234,25 @@ int main() { 45.0, 60.0, 80.0, + 89.91, 89.99, 90.0}; const Vector lons{ -180.0, -135.0, -90.0, -45.0, 0.0, 45.0, 90.0, 135.0, 180.0}; + const Vector zas{0.0, + 0.01, + 1.0, + 15.0, + 30.0, + 45.0, + 60.0, + 90.0, + 120.0, + 150.0, + 179.0, + 179.99, + 180.0}; + const Vector aas{0.0, 45.0, 90.0, 135.0, 180.0, 225.0, 270.0, 315.0, 360.0}; Stats stats_wgs84_high; // WGS-84 overall for high altitude (>100km) Stats stats_wgs84_low; // WGS-84 overall for low altitude (<=100km) @@ -182,6 +271,27 @@ int main() { Stats stats_sphere_rt_low; // Sphere round-trip for low altitude (<=100km) Stats stats_sphere_rt_high; // Sphere round-trip for high altitude (>100km) + // LOS old-vs-new stats + LosStats stats_los_low; + LosStats stats_los_high; + LosStats stats_los_poles_low; + LosStats stats_los_poles_high; + LosStats stats_los_mid_low; + LosStats stats_los_mid_high; + LosStats stats_los_at_zenith_low; + LosStats stats_los_at_zenith_high; + + // LOS round-trip stats + LosStats stats_los_rt_low; // LOS round-trip for low altitude (<=100km) + LosStats stats_los_rt_high; // LOS round-trip for high altitude (>100km) + LosStats stats_los_poles_rt_low; // Near-pole LOS round-trip for low altitude + LosStats + stats_los_poles_rt_high; // Near-pole LOS round-trip for high altitude + LosStats stats_los_mid_rt_low; // Mid-lat LOS round-trip for low altitude + LosStats stats_los_mid_rt_high; // Mid-lat LOS round-trip for high altitude + LosStats stats_los_at_zenith_rt_low; // za=0/180 LOS round-trip low alt + LosStats stats_los_at_zenith_rt_high; // za=0/180 LOS round-trip high alt + // --- Accuracy test --- for (Numeric h : alts) { for (Numeric lat : lats) { @@ -240,6 +350,71 @@ int main() { update_stats(stats_sphere_rt_high, geo, new_s); else update_stats(stats_sphere_rt_low, geo, new_s); + + for (Numeric za : zas) { + for (Numeric aa : aas) { + Vector2 los{za, aa}; + auto [ecef_los, decef_los] = geodetic_los2ecef(geo, los, wgs84); + + bool is_at_zenith = (za == 0.0 || za == 180.0); + + // Accuracy comparison: old vs new ecef2geodetic_los + auto [pos_old, los_old] = ecef2geodetic_los_with_ecef2geodetic_old( + ecef_los, decef_los, wgs84); + auto [pos_new, los_new] = + ecef2geodetic_los(ecef_los, decef_los, wgs84); + if (is_at_zenith) { + if (is_high_altitude) + update_los_stats(stats_los_at_zenith_high, los_old, los_new); + else + update_los_stats(stats_los_at_zenith_low, los_old, los_new); + } else { + if (is_high_altitude) { + update_los_stats(stats_los_high, los_old, los_new); + if (std::abs(lat) > 89.9) { + update_los_stats(stats_los_poles_high, los_old, los_new); + } else if (std::abs(lat) < 80.0) { + update_los_stats(stats_los_mid_high, los_old, los_new); + } + } else { + update_los_stats(stats_los_low, los_old, los_new); + if (std::abs(lat) > 89.9) { + update_los_stats(stats_los_poles_low, los_old, los_new); + } else if (std::abs(lat) < 80.0) { + update_los_stats(stats_los_mid_low, los_old, los_new); + } + } + } + + // LOS round-trip test + auto [geo_rt, los_rt] = + ecef2geodetic_los(ecef_los, decef_los, wgs84); + + if (is_at_zenith) { + if (is_high_altitude) + update_los_stats(stats_los_at_zenith_rt_high, los, los_rt); + else + update_los_stats(stats_los_at_zenith_rt_low, los, los_rt); + } else { + if (is_high_altitude) + update_los_stats(stats_los_rt_high, los, los_rt); + else + update_los_stats(stats_los_rt_low, los, los_rt); + + if (std::abs(lat) > 89.9) { + if (is_high_altitude) + update_los_stats(stats_los_poles_rt_high, los, los_rt); + else + update_los_stats(stats_los_poles_rt_low, los, los_rt); + } else if (std::abs(lat) < 80.0) { + if (is_high_altitude) + update_los_stats(stats_los_mid_rt_high, los, los_rt); + else + update_los_stats(stats_los_mid_rt_low, los, los_rt); + } + } + } + } } } } @@ -268,6 +443,37 @@ int main() { print_stats(stats_sphere_rt_low, "Sphere round-trip (low altitude <=100km)"); print_stats(stats_sphere_rt_high, "Sphere round-trip (high altitude >100km)"); + std::println( + "=== LOS accuracy comparison (ecef2geodetic_los_old vs ecef2geodetic_los) ==="); + print_los_stats(stats_los_low, "LOS (low altitude <=100km)"); + print_los_stats(stats_los_high, "LOS (high altitude >100km)"); + print_los_stats(stats_los_poles_low, "Near-pole LOS (low altitude <=100km)"); + print_los_stats(stats_los_poles_high, "Near-pole LOS (high altitude >100km)"); + print_los_stats(stats_los_mid_low, "Mid-latitude LOS (low altitude <=100km)"); + print_los_stats(stats_los_mid_high, + "Mid-latitude LOS (high altitude >100km)"); + print_los_stats(stats_los_at_zenith_low, + "LOS at zenith/nadir (low altitude <=100km)"); + print_los_stats(stats_los_at_zenith_high, + "LOS at zenith/nadir (high altitude >100km)"); + + std::println( + "=== LOS round-trip error (geodetic_los2ecef -> ecef2geodetic_los) ==="); + print_los_stats(stats_los_rt_low, "LOS round-trip (low altitude <=100km)"); + print_los_stats(stats_los_rt_high, "LOS round-trip (high altitude >100km)"); + print_los_stats(stats_los_poles_rt_low, + "Near-pole LOS round-trip (low altitude <=100km)"); + print_los_stats(stats_los_poles_rt_high, + "Near-pole LOS round-trip (high altitude >100km)"); + print_los_stats(stats_los_mid_rt_low, + "Mid-latitude LOS round-trip (low altitude <=100km)"); + print_los_stats(stats_los_mid_rt_high, + "Mid-latitude LOS round-trip (high altitude >100km)"); + print_los_stats(stats_los_at_zenith_rt_low, + "LOS at zenith/nadir round-trip (low altitude <=100km)"); + print_los_stats(stats_los_at_zenith_rt_high, + "LOS at zenith/nadir round-trip (high altitude >100km)"); + // --- Performance test --- constexpr Index n_perf = 1'000'000;