From ef7269cc09f3a6086677e2503566c40b1008a86a Mon Sep 17 00:00:00 2001 From: Ryan Pavlick <218223+ryanpavlick@users.noreply.github.com> Date: Sun, 26 Apr 2026 22:01:29 -0400 Subject: [PATCH] Adds ECI/ECEF state transforms --- src/pymap3d/__init__.py | 13 +- src/pymap3d/earth_orientation.py | 33 ++++ src/pymap3d/eci.py | 315 ++++++++++++++++++++++++++++++- src/pymap3d/tests/test_eci.py | 58 ++++++ 4 files changed, 413 insertions(+), 6 deletions(-) diff --git a/src/pymap3d/__init__.py b/src/pymap3d/__init__.py index 5d496b2..be09caf 100644 --- a/src/pymap3d/__init__.py +++ b/src/pymap3d/__init__.py @@ -178,6 +178,13 @@ from .aer import aer2eci, eci2aer from .azelradec import azel2radec, radec2azel -from .eci import ecef2eci, eci2ecef - -__all__ += ["aer2eci", "eci2aer", "ecef2eci", "eci2ecef"] +from .eci import ecef2eci, ecef2eci_state, eci2ecef, eci2ecef_state + +__all__ += [ + "aer2eci", + "eci2aer", + "ecef2eci", + "eci2ecef", + "ecef2eci_state", + "eci2ecef_state", +] diff --git a/src/pymap3d/earth_orientation.py b/src/pymap3d/earth_orientation.py index fba9595..e649409 100644 --- a/src/pymap3d/earth_orientation.py +++ b/src/pymap3d/earth_orientation.py @@ -229,6 +229,18 @@ def matmul3(a, b): ) +def matsub3(a, b): + """3x3 matrix subtraction.""" + + return tuple(tuple(a[i][j] - b[i][j] for j in range(3)) for i in range(3)) + + +def matscale3(a, scale: float): + """3x3 matrix scalar multiply.""" + + return tuple(tuple(scale * a[i][j] for j in range(3)) for i in range(3)) + + def matvec3(a, v): """3x3 matrix by 3-vector.""" @@ -300,3 +312,24 @@ def eci_to_ecef_matrix( pm = polar_motion_matrix(xp, yp) return matmul3(pm, matmul3(st, matmul3(n, p))) + + +def eci_to_ecef_matrix_rate( + time: datetime, + delta_ut1: float = 0.0, + xp: float = 0.0, + yp: float = 0.0, + dt_seconds: float = 0.5, +): + """ + Time derivative of the ECI->ECEF rotation matrix. + + Uses a central difference over the same Earth-orientation model as + ``eci_to_ecef_matrix()``. + """ + + dt = timedelta(seconds=dt_seconds) + c_plus = eci_to_ecef_matrix(time + dt, delta_ut1=delta_ut1, xp=xp, yp=yp) + c_minus = eci_to_ecef_matrix(time - dt, delta_ut1=delta_ut1, xp=xp, yp=yp) + + return matscale3(matsub3(c_plus, c_minus), 1.0 / (2.0 * dt_seconds)) diff --git a/src/pymap3d/eci.py b/src/pymap3d/eci.py index 6cc2a8b..7c7b232 100644 --- a/src/pymap3d/eci.py +++ b/src/pymap3d/eci.py @@ -9,13 +9,19 @@ try: import numpy import astropy.units as u - from astropy.coordinates import GCRS, ITRS, CartesianRepresentation, EarthLocation + from astropy.coordinates import ( + GCRS, + ITRS, + CartesianDifferential, + CartesianRepresentation, + EarthLocation, + ) except ImportError: pass -from .earth_orientation import eci_to_ecef_matrix, matvec3, transpose3 +from .earth_orientation import eci_to_ecef_matrix, eci_to_ecef_matrix_rate, matvec3, transpose3 -__all__ = ["eci2ecef", "ecef2eci"] +__all__ = ["eci2ecef", "ecef2eci", "eci2ecef_state", "ecef2eci_state"] def eci2ecef( @@ -199,6 +205,208 @@ def ecef2eci_numpy( return _squeeze_xyz(*_apply_rotation(rotation, x, y, z)) +def eci2ecef_state( + x, + y, + z, + vx, + vy, + vz, + time: datetime, + force_non_astropy: bool = False, + *, + delta_ut1: float = 0.0, + xp: float = 0.0, + yp: float = 0.0, +) -> tuple: + """ + Transform an ECI position/velocity state to ECEF. + + Velocity transforms depend on both position and velocity because ECEF rotates + with the Earth. + """ + + if "astropy" in sys.modules and not force_non_astropy: + state = eci2ecef_state_astropy(x, y, z, vx, vy, vz, time) + else: + logging.warning( + f"{__name__}: using pure-Python ECI/ECEF fallback, less accurate than Astropy" + ) + state = eci2ecef_state_numpy( + x, + y, + z, + vx, + vy, + vz, + time, + delta_ut1=delta_ut1, + xp=xp, + yp=yp, + ) + + return _squeeze_state(*state) + + +def eci2ecef_state_astropy(x, y, z, vx, vy, vz, t: datetime) -> tuple: + """eci2ecef_state using Astropy.""" + + gcrs = GCRS( + CartesianRepresentation( + x * u.m, + y * u.m, + z * u.m, + differentials=CartesianDifferential(vx * u.m / u.s, vy * u.m / u.s, vz * u.m / u.s), + ), + obstime=t, + ) + itrs = gcrs.transform_to(ITRS(obstime=t)) + rep = itrs.cartesian + diff = rep.differentials["s"] + + return ( + rep.x.to_value(u.m), + rep.y.to_value(u.m), + rep.z.to_value(u.m), + diff.d_x.to_value(u.m / u.s), + diff.d_y.to_value(u.m / u.s), + diff.d_z.to_value(u.m / u.s), + ) + + +def eci2ecef_state_numpy( + x, + y, + z, + vx, + vy, + vz, + t: datetime, + *, + delta_ut1: float = 0.0, + xp: float = 0.0, + yp: float = 0.0, +) -> tuple: + """eci2ecef_state using the internal pure-Python Earth orientation model.""" + + rotation = eci_to_ecef_matrix(t, delta_ut1=delta_ut1, xp=xp, yp=yp) + rotation_rate = eci_to_ecef_matrix_rate(t, delta_ut1=delta_ut1, xp=xp, yp=yp) + return _squeeze_state( + *_apply_state_rotation( + rotation, + rotation_rate, + x, + y, + z, + vx, + vy, + vz, + inverse=False, + ) + ) + + +def ecef2eci_state( + x, + y, + z, + vx, + vy, + vz, + time: datetime, + force_non_astropy: bool = False, + *, + delta_ut1: float = 0.0, + xp: float = 0.0, + yp: float = 0.0, +) -> tuple: + """ + Transform an ECEF position/velocity state to ECI. + + Velocity transforms depend on both position and velocity because ECEF rotates + with the Earth. + """ + + if "astropy" in sys.modules and not force_non_astropy: + state = ecef2eci_state_astropy(x, y, z, vx, vy, vz, time) + else: + logging.warning( + f"{__name__}: using pure-Python ECI/ECEF fallback, less accurate than Astropy" + ) + state = ecef2eci_state_numpy( + x, + y, + z, + vx, + vy, + vz, + time, + delta_ut1=delta_ut1, + xp=xp, + yp=yp, + ) + + return _squeeze_state(*state) + + +def ecef2eci_state_astropy(x, y, z, vx, vy, vz, t: datetime) -> tuple: + """ecef2eci_state using Astropy.""" + + itrs = ITRS( + CartesianRepresentation( + x * u.m, + y * u.m, + z * u.m, + differentials=CartesianDifferential(vx * u.m / u.s, vy * u.m / u.s, vz * u.m / u.s), + ), + obstime=t, + ) + gcrs = itrs.transform_to(GCRS(obstime=t)) + rep = gcrs.cartesian + diff = rep.differentials["s"] + + return ( + rep.x.to_value(u.m), + rep.y.to_value(u.m), + rep.z.to_value(u.m), + diff.d_x.to_value(u.m / u.s), + diff.d_y.to_value(u.m / u.s), + diff.d_z.to_value(u.m / u.s), + ) + + +def ecef2eci_state_numpy( + x, + y, + z, + vx, + vy, + vz, + t: datetime, + *, + delta_ut1: float = 0.0, + xp: float = 0.0, + yp: float = 0.0, +) -> tuple: + """ecef2eci_state using the internal pure-Python Earth orientation model.""" + + rotation = eci_to_ecef_matrix(t, delta_ut1=delta_ut1, xp=xp, yp=yp) + rotation_rate = eci_to_ecef_matrix_rate(t, delta_ut1=delta_ut1, xp=xp, yp=yp) + return _squeeze_state( + *_apply_state_rotation( + rotation, + rotation_rate, + x, + y, + z, + vx, + vy, + vz, + inverse=True, + ) + ) + + def _apply_rotation(rotation, x, y, z): """Apply a 3x3 rotation to scalar or NumPy coordinate inputs.""" @@ -224,6 +432,91 @@ def _apply_rotation(rotation, x, y, z): ) +def _apply_state_rotation( + rotation, + rotation_rate, + x, + y, + z, + vx, + vy, + vz, + *, + inverse: bool, +): + """Apply a position/velocity frame transform.""" + + if "numpy" not in sys.modules: + return _apply_state_rotation_scalar( + rotation, rotation_rate, x, y, z, vx, vy, vz, inverse=inverse + ) + + x = numpy.atleast_1d(x) + y = numpy.atleast_1d(y) + z = numpy.atleast_1d(z) + vx = numpy.atleast_1d(vx) + vy = numpy.atleast_1d(vy) + vz = numpy.atleast_1d(vz) + assert x.shape == y.shape == z.shape == vx.shape == vy.shape == vz.shape, ( + "shape mismatch between position and velocity inputs" + ) + + pos_in = numpy.column_stack((x.ravel(), y.ravel(), z.ravel())) + vel_in = numpy.column_stack((vx.ravel(), vy.ravel(), vz.ravel())) + pos_out = numpy.empty((x.size, 3)) + vel_out = numpy.empty((x.size, 3)) + + for i in range(pos_in.shape[0]): + pos_i, vel_i = _apply_state_rotation_scalar( + rotation, + rotation_rate, + *tuple(pos_in[i, :]), + *tuple(vel_in[i, :]), + inverse=inverse, + ) + pos_out[i, :] = pos_i + vel_out[i, :] = vel_i + + return ( + pos_out[:, 0].reshape(x.shape), + pos_out[:, 1].reshape(y.shape), + pos_out[:, 2].reshape(z.shape), + vel_out[:, 0].reshape(vx.shape), + vel_out[:, 1].reshape(vy.shape), + vel_out[:, 2].reshape(vz.shape), + ) + + +def _apply_state_rotation_scalar( + rotation, + rotation_rate, + x, + y, + z, + vx, + vy, + vz, + *, + inverse: bool, +): + """Scalar state rotation helper.""" + + if inverse: + position = matvec3(transpose3(rotation), (x, y, z)) + coriolis = matvec3(rotation_rate, position) + velocity = matvec3( + transpose3(rotation), + (vx - coriolis[0], vy - coriolis[1], vz - coriolis[2]), + ) + else: + position = matvec3(rotation, (x, y, z)) + spin = matvec3(rotation_rate, (x, y, z)) + rotated_velocity = matvec3(rotation, (vx, vy, vz)) + velocity = tuple(rotated_velocity[i] + spin[i] for i in range(3)) + + return position, velocity + + def _squeeze_xyz(x, y, z): """Preserve scalar inputs while supporting array outputs.""" @@ -231,3 +524,19 @@ def _squeeze_xyz(x, y, z): return x.squeeze()[()], y.squeeze()[()], z.squeeze()[()] except AttributeError: return x, y, z + + +def _squeeze_state(x, y, z, vx, vy, vz): + """Preserve scalar state inputs while supporting array outputs.""" + + try: + return ( + x.squeeze()[()], + y.squeeze()[()], + z.squeeze()[()], + vx.squeeze()[()], + vy.squeeze()[()], + vz.squeeze()[()], + ) + except AttributeError: + return x, y, z, vx, vy, vz diff --git a/src/pymap3d/tests/test_eci.py b/src/pymap3d/tests/test_eci.py index 118b833..237ffbd 100644 --- a/src/pymap3d/tests/test_eci.py +++ b/src/pymap3d/tests/test_eci.py @@ -11,6 +11,7 @@ ECI = (-2981784, 5207055, 3161595) ECEF = [-5762640, -1682738, 3156028] +ECI_STATE = (-2981784, 5207055, 3161595, 100.0, -250.0, 50.0) UTC = datetime.datetime(2019, 1, 4, 12, tzinfo=datetime.timezone.utc) @@ -79,6 +80,41 @@ def test_ecef2eci_astropy(): assert isinstance(eci[2], float) +def test_eci2ecef_state(): + state = pm.eci2ecef_state(*ECI_STATE, UTC) + + assert all(isinstance(component, float) for component in state) + + +def test_ecef2eci_state(): + pytest.importorskip("numpy") + + state = pm.ecef2eci_state(*ECEF, 100.0, -250.0, 50.0, UTC) + + assert all(isinstance(component, float) for component in state) + + +def test_eci2ecef_state_numpy(): + pytest.importorskip("astropy") + + state = pm.eci2ecef_state(*ECI_STATE, UTC, force_non_astropy=True) + ref = pm.eci2ecef_state(*ECI_STATE, UTC) + + assert state[:3] == approx(ref[:3], abs=60) + assert state[3:] == approx(ref[3:], abs=0.05) + + +def test_ecef2eci_state_numpy(): + pytest.importorskip("astropy") + + ecef_state = pm.eci2ecef_state(*ECI_STATE, UTC) + state = pm.ecef2eci_state(*ecef_state, UTC, force_non_astropy=True) + ref = pm.ecef2eci_state(*ecef_state, UTC) + + assert state[:3] == approx(ref[:3], abs=60) + assert state[3:] == approx(ref[3:], abs=0.05) + + def test_eci2geodetic(): lla = pm.eci2geodetic(*ECI, UTC) @@ -106,6 +142,28 @@ def test_eci_ecef_eop_roundtrip(): assert eci == approx(ECI, abs=1e-6) +def test_eci_ecef_state_eop_roundtrip(): + eop = {"delta_ut1": -0.139198, "xp": 0.094, "yp": 0.324} + + ecef_state = pm.eci2ecef_state(*ECI_STATE, UTC, force_non_astropy=True, **eop) + eci_state = pm.ecef2eci_state(*ecef_state, UTC, force_non_astropy=True, **eop) + + assert eci_state == approx(ECI_STATE, abs=1e-6) + + +def test_stationary_ecef_point_has_nonzero_eci_velocity(): + xyz = pm.geodetic2ecef(28, -80, 100) + + eci_state = pm.ecef2eci_state(*xyz, 0.0, 0.0, 0.0, UTC, force_non_astropy=True) + + assert abs(eci_state[3]) > 100 + assert abs(eci_state[4]) > 100 + + ecef_state = pm.eci2ecef_state(*eci_state, UTC, force_non_astropy=True) + + assert ecef_state == approx((*xyz, 0.0, 0.0, 0.0), abs=1e-6) + + def test_eci_aer(): # test coords from Matlab eci2aer pytest.importorskip("numpy")