mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-22 08:56:37 +00:00
Yuzviak
f66b266219
Four I001 violations surfaced when running ruff over the full src/ tests/ tree (the CI command) rather than just the testing subpath: - src/gps_denied/testing/coord.py - src/gps_denied/testing/datasets/vpair.py - tests/e2e/test_coord.py - tests/e2e/test_vpair_adapter.py All auto-fixable; no behavioural change. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
76 lines
2.7 KiB
Python
76 lines
2.7 KiB
Python
"""Coordinate conversion helpers used by dataset adapters.
|
|
|
|
- `ecef_to_wgs84`: closed-form Zhu/Heikkinen conversion (no iteration), accurate
|
|
to centimetres for any point above the Earth's surface.
|
|
- `euler_to_quaternion`: aerospace ZYX intrinsic convention (yaw → pitch → roll
|
|
applied to body frame in that order). Inputs in radians.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
|
|
# WGS84 constants
|
|
_A = 6_378_137.0 # semi-major axis, metres
|
|
_F = 1.0 / 298.257223563 # flattening
|
|
_B = _A * (1.0 - _F) # semi-minor axis
|
|
_E2 = 1.0 - (_B * _B) / (_A * _A) # first eccentricity squared
|
|
_EP2 = (_A * _A) / (_B * _B) - 1.0 # second eccentricity squared
|
|
|
|
|
|
def ecef_to_wgs84(x: float, y: float, z: float) -> tuple[float, float, float]:
|
|
"""ECEF (metres) → WGS84 (lat° N, lon° E, altitude metres above ellipsoid).
|
|
|
|
Uses Heikkinen's closed-form method — no iteration, one pass, centimetre
|
|
accuracy for any realistic aerial vehicle altitude.
|
|
"""
|
|
r = math.sqrt(x * x + y * y)
|
|
if r == 0.0:
|
|
# Pole: lon is conventionally 0, lat is ±90 depending on sign of z
|
|
lat = 90.0 if z >= 0 else -90.0
|
|
alt = abs(z) - _B
|
|
return lat, 0.0, alt
|
|
|
|
ee = _A * _A - _B * _B
|
|
f = 54.0 * _B * _B * z * z
|
|
g = r * r + (1.0 - _E2) * z * z - _E2 * ee
|
|
c = (_E2 * _E2 * f * r * r) / (g * g * g)
|
|
s = (1.0 + c + math.sqrt(c * c + 2.0 * c)) ** (1.0 / 3.0)
|
|
p = f / (3.0 * (s + 1.0 / s + 1.0) ** 2 * g * g)
|
|
q = math.sqrt(1.0 + 2.0 * _E2 * _E2 * p)
|
|
r0 = -(p * _E2 * r) / (1.0 + q) + math.sqrt(
|
|
0.5 * _A * _A * (1.0 + 1.0 / q)
|
|
- (p * (1.0 - _E2) * z * z) / (q * (1.0 + q))
|
|
- 0.5 * p * r * r
|
|
)
|
|
u = math.sqrt((r - _E2 * r0) ** 2 + z * z)
|
|
v = math.sqrt((r - _E2 * r0) ** 2 + (1.0 - _E2) * z * z)
|
|
z0 = (_B * _B * z) / (_A * v)
|
|
|
|
lat = math.degrees(math.atan((z + _EP2 * z0) / r))
|
|
lon = math.degrees(math.atan2(y, x))
|
|
alt = u * (1.0 - (_B * _B) / (_A * v))
|
|
return lat, lon, alt
|
|
|
|
|
|
def euler_to_quaternion(
|
|
roll: float, pitch: float, yaw: float
|
|
) -> tuple[float, float, float, float]:
|
|
"""Aerospace ZYX intrinsic Euler → (qx, qy, qz, qw).
|
|
|
|
Rotation order: yaw (about world z) → pitch (about intermediate y) → roll
|
|
(about body x). All inputs in radians. Output quaternion has unit norm.
|
|
"""
|
|
cr = math.cos(roll * 0.5)
|
|
sr = math.sin(roll * 0.5)
|
|
cp = math.cos(pitch * 0.5)
|
|
sp = math.sin(pitch * 0.5)
|
|
cy = math.cos(yaw * 0.5)
|
|
sy = math.sin(yaw * 0.5)
|
|
|
|
qw = cr * cp * cy + sr * sp * sy
|
|
qx = sr * cp * cy - cr * sp * sy
|
|
qy = cr * sp * cy + sr * cp * sy
|
|
qz = cr * cp * sy - sr * sp * cy
|
|
return qx, qy, qz, qw
|