From 87ab81e7d094064a1a6e8b75c1d4354888be99a4 Mon Sep 17 00:00:00 2001 From: Ryan Malloy Date: Tue, 17 Feb 2026 15:44:48 -0700 Subject: [PATCH] Add observation-to-TLE fitting (orbit determination) for v0.4.0 Batch weighted least-squares differential correction using equinoctial elements, LAPACK dgelss_() for SVD solve, vendored SGP4/SDP4 as the propagation engine. Per Vallado & Crawford (2008) AIAA 2008-6770. New SQL functions: - tle_from_eci(): fit TLE from ECI position/velocity ephemeris - tle_from_topocentric(): fit TLE from az/el/range observations - tle_fit_residuals(): per-observation position residuals diagnostic Solver features: 6-state (orbital) or 7-state (+ B*) fitting, equinoctial elements for singularity-free optimization, tiered step limiting, Brouwer/Kozai Newton-Raphson conversion, auto initial guess from first ECI observation when no seed TLE provided. Tested: 8 regression tests (LEO/MEO/near-circular round-trips, B* recovery, topocentric, seedless, error handling, diagnostics), 67 standalone math unit tests, all 14 suites pass. --- .gitignore | 1 + Dockerfile | 13 +- Makefile | 20 +- TODO | 10 + pg_orrery.control | 2 +- sql/pg_orrery--0.3.0--0.4.0.sql | 64 +++ sql/pg_orrery--0.4.0.sql | 854 ++++++++++++++++++++++++++++++++ src/od_funcs.c | 483 ++++++++++++++++++ src/od_math.c | 513 +++++++++++++++++++ src/od_math.h | 141 ++++++ src/od_solver.c | 760 ++++++++++++++++++++++++++++ src/od_solver.h | 100 ++++ test/expected/od_fit.out | 259 ++++++++++ test/sql/od_fit.sql | 239 +++++++++ test/test_od_math.c | 464 +++++++++++++++++ 15 files changed, 3914 insertions(+), 9 deletions(-) create mode 100644 TODO create mode 100644 sql/pg_orrery--0.3.0--0.4.0.sql create mode 100644 sql/pg_orrery--0.4.0.sql create mode 100644 src/od_funcs.c create mode 100644 src/od_math.c create mode 100644 src/od_math.h create mode 100644 src/od_solver.c create mode 100644 src/od_solver.h create mode 100644 test/expected/od_fit.out create mode 100644 test/sql/od_fit.sql create mode 100644 test/test_od_math.c diff --git a/.gitignore b/.gitignore index f474074..365916c 100644 --- a/.gitignore +++ b/.gitignore @@ -21,6 +21,7 @@ log/ # Test artifacts test/matrix-logs/ test/test_de_reader +test/test_od_math # Docs site docs/node_modules/ diff --git a/Dockerfile b/Dockerfile index c5128de..2798449 100644 --- a/Dockerfile +++ b/Dockerfile @@ -20,7 +20,8 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ apt-get update && apt-get install -y --no-install-recommends \ postgresql-${PG_MAJOR} \ postgresql-server-dev-${PG_MAJOR} \ - gcc make && \ + gcc make \ + liblapack-dev libblas-dev && \ rm -rf /var/lib/apt/lists/* # Copy source tree (submodule content included as regular files) @@ -35,15 +36,16 @@ RUN make PG_CONFIG=${PG_CONFIG} # Install to system location (needed for installcheck) RUN make PG_CONFIG=${PG_CONFIG} install -# Run all 13 regression test suites against a throwaway cluster +# Run all 14 regression test suites against a throwaway cluster RUN su postgres -c "/usr/lib/postgresql/${PG_MAJOR}/bin/initdb -D /tmp/pgtest" && \ su postgres -c "/usr/lib/postgresql/${PG_MAJOR}/bin/pg_ctl -D /tmp/pgtest -l /tmp/pgtest.log start" && \ su postgres -c "/usr/lib/postgresql/${PG_MAJOR}/bin/createuser -s root" && \ make PG_CONFIG=${PG_CONFIG} installcheck && \ su postgres -c "/usr/lib/postgresql/${PG_MAJOR}/bin/pg_ctl -D /tmp/pgtest stop" -# Standalone DE reader unit test (no PostgreSQL dependency) +# Standalone unit tests (no PostgreSQL dependency) RUN make test-de-reader +RUN make test-od-math # Capture artifacts under /pg_orrery prefix for the next stage RUN make PG_CONFIG=${PG_CONFIG} DESTDIR=/pg_orrery install @@ -71,6 +73,11 @@ COPY docker/020_install_pg_orrery.sh /docker-entrypoint-initdb.d/ # docker build --build-arg DE_FILE=de440.bin -t pg_orrery:de440 . FROM postgres:${PG_MAJOR}-bookworm AS standalone +# LAPACK/BLAS runtime for OD solver (dgelss_) +RUN apt-get update && apt-get install -y --no-install-recommends \ + liblapack3 libblas3 && \ + rm -rf /var/lib/apt/lists/* + COPY --from=artifact / / # Create the pg_orrery data directory for DE ephemeris files diff --git a/Makefile b/Makefile index ed0d108..d270f69 100644 --- a/Makefile +++ b/Makefile @@ -1,7 +1,8 @@ MODULE_big = pg_orrery EXTENSION = pg_orrery DATA = sql/pg_orrery--0.1.0.sql sql/pg_orrery--0.2.0.sql sql/pg_orrery--0.1.0--0.2.0.sql \ - sql/pg_orrery--0.3.0.sql sql/pg_orrery--0.2.0--0.3.0.sql + sql/pg_orrery--0.3.0.sql sql/pg_orrery--0.2.0--0.3.0.sql \ + sql/pg_orrery--0.4.0.sql sql/pg_orrery--0.3.0--0.4.0.sql # Our extension C sources OBJS = src/pg_orrery.o src/tle_type.o src/eci_type.o src/observer_type.o \ @@ -12,7 +13,8 @@ OBJS = src/pg_orrery.o src/tle_type.o src/eci_type.o src/observer_type.o \ src/tass17.o src/gust86.o src/marssat.o src/l12.o \ src/moon_funcs.o src/radio_funcs.o \ src/lambert.o src/transfer_funcs.o \ - src/de_reader.o src/eph_provider.o src/de_funcs.o + src/de_reader.o src/eph_provider.o src/de_funcs.o \ + src/od_math.o src/od_solver.o src/od_funcs.o # Vendored SGP4/SDP4 sources (pure C, from Bill Gray's sat_code, MIT license) SGP4_DIR = src/sgp4 @@ -27,11 +29,11 @@ OBJS += $(SGP4_OBJS) # Regression tests REGRESS = tle_parse sgp4_propagate coord_transforms pass_prediction gist_index convenience \ star_observe kepler_comet planet_observe moon_observe lambert_transfer \ - de_ephemeris vallado_518 + de_ephemeris od_fit vallado_518 REGRESS_OPTS = --inputdir=test -# Pure C — no C++ runtime needed -SHLIB_LINK += -lm +# Pure C — no C++ runtime needed. LAPACK for OD solver (dgelss_). +SHLIB_LINK += -lm -llapack -lblas # Compiler flags PG_CPPFLAGS = -I$(SGP4_DIR) @@ -50,6 +52,14 @@ test-de-reader: test/test_de_reader.c src/de_reader.c src/de_reader.h .PHONY: test-de-reader +# ── Standalone OD math unit test (no PostgreSQL dependency) ── +# Element converters, inverse coordinate transforms, Brouwer/Kozai inverse. +test-od-math: test/test_od_math.c src/od_math.c src/od_math.h + $(CC) -Wall -Werror -Isrc -o test/test_od_math $< src/od_math.c -lm + ./test/test_od_math + +.PHONY: test-od-math + # ── PG version test matrix ───────────────────────────────── PG_TEST_VERSIONS ?= 14 15 16 17 18 diff --git a/TODO b/TODO new file mode 100644 index 0000000..ada691e --- /dev/null +++ b/TODO @@ -0,0 +1,10 @@ + + - IOD bootstrap: Gauss/Gibbs/double-r methods to generate + initial TLE from angles-only (eliminates seed requirement) + - Covariance output: Return (A^TWA)^{-1} matrix for + uncertainty estimates / conjunction screening + - Multi-observer: Accept observations from multiple ground + stations + - Adaptive step limiting: Dynamically tune correction limits + per Vallado's observation about different strategies failing + on different satellite subsets diff --git a/pg_orrery.control b/pg_orrery.control index 34df8c8..89b48bc 100644 --- a/pg_orrery.control +++ b/pg_orrery.control @@ -1,4 +1,4 @@ comment = 'A database orrery — celestial mechanics types and functions for PostgreSQL' -default_version = '0.3.0' +default_version = '0.4.0' module_pathname = '$libdir/pg_orrery' relocatable = true diff --git a/sql/pg_orrery--0.3.0--0.4.0.sql b/sql/pg_orrery--0.3.0--0.4.0.sql new file mode 100644 index 0000000..201452f --- /dev/null +++ b/sql/pg_orrery--0.3.0--0.4.0.sql @@ -0,0 +1,64 @@ +-- pg_orrery 0.3.0 -> 0.4.0 migration +-- +-- Adds observation-to-TLE fitting via batch weighted least-squares +-- differential correction (Vallado & Crawford 2008, AIAA 2008-6770). +-- Uses equinoctial elements internally for singularity-free optimization. +-- LAPACK dgelss_() for SVD solve. + +-- ============================================================ +-- Phase 6: Orbit determination (TLE fitting from observations) +-- ============================================================ + +-- Fit TLE from ECI position/velocity ephemeris + +CREATE FUNCTION tle_from_eci( + positions eci_position[], + times timestamptz[], + seed tle DEFAULT NULL, + fit_bstar boolean DEFAULT false, + max_iter int4 DEFAULT 15, + OUT fitted_tle tle, + OUT iterations int4, + OUT rms_final float8, + OUT rms_initial float8, + OUT status text +) RETURNS RECORD + AS 'MODULE_PATHNAME' LANGUAGE C STABLE PARALLEL SAFE; +COMMENT ON FUNCTION tle_from_eci(eci_position[], timestamptz[], tle, boolean, int4) IS + 'Fit a TLE from ECI position/velocity observations via differential correction. Returns fitted TLE, iteration count, RMS residuals, and convergence status. Requires >= 6 observations.'; + +-- Fit TLE from topocentric observations (az/el/range) + +CREATE FUNCTION tle_from_topocentric( + observations topocentric[], + times timestamptz[], + obs observer, + seed tle DEFAULT NULL, + fit_bstar boolean DEFAULT false, + max_iter int4 DEFAULT 15, + OUT fitted_tle tle, + OUT iterations int4, + OUT rms_final float8, + OUT rms_initial float8, + OUT status text +) RETURNS RECORD + AS 'MODULE_PATHNAME' LANGUAGE C STABLE PARALLEL SAFE; +COMMENT ON FUNCTION tle_from_topocentric(topocentric[], timestamptz[], observer, tle, boolean, int4) IS + 'Fit a TLE from topocentric (az/el/range) observations via differential correction. Requires seed TLE and >= 6 observations.'; + +-- Per-observation residuals diagnostic + +CREATE FUNCTION tle_fit_residuals( + fitted tle, + positions eci_position[], + times timestamptz[] +) RETURNS TABLE ( + t timestamptz, + dx_km float8, + dy_km float8, + dz_km float8, + pos_err_km float8 +) + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION tle_fit_residuals(tle, eci_position[], timestamptz[]) IS + 'Compute per-observation position residuals (km) between a TLE and ECI observations. Useful for fit quality assessment.'; diff --git a/sql/pg_orrery--0.4.0.sql b/sql/pg_orrery--0.4.0.sql new file mode 100644 index 0000000..ae1c7d4 --- /dev/null +++ b/sql/pg_orrery--0.4.0.sql @@ -0,0 +1,854 @@ +-- pg_orrery -- Orbital mechanics types and functions for PostgreSQL +-- +-- Types: tle, eci_position, geodetic, topocentric, observer, pass_event +-- Provides SGP4/SDP4 propagation, coordinate transforms, pass prediction, +-- and GiST indexing on altitude bands for conjunction screening. +-- +-- All propagation uses WGS-72 constants (matching TLE mean element fitting). +-- Coordinate output uses WGS-84 (matching modern geodetic standards). + +-- ============================================================ +-- Shell types (forward declarations) +-- ============================================================ + +CREATE TYPE tle; +CREATE TYPE eci_position; +CREATE TYPE geodetic; +CREATE TYPE topocentric; +CREATE TYPE observer; +CREATE TYPE pass_event; + + +-- ============================================================ +-- TLE type: Two-Line Element set +-- ============================================================ + +CREATE FUNCTION tle_in(cstring) RETURNS tle + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION tle_out(tle) RETURNS cstring + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION tle_recv(internal) RETURNS tle + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION tle_send(tle) RETURNS bytea + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE TYPE tle ( + INPUT = tle_in, + OUTPUT = tle_out, + RECEIVE = tle_recv, + SEND = tle_send, + INTERNALLENGTH = 112, + ALIGNMENT = double, + STORAGE = plain +); + +COMMENT ON TYPE tle IS 'Two-Line Element set — parsed mean orbital elements for SGP4/SDP4 propagation'; + +-- TLE accessor functions + +CREATE FUNCTION tle_epoch(tle) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION tle_epoch(tle) IS 'TLE epoch as Julian date (UTC)'; + +CREATE FUNCTION tle_norad_id(tle) RETURNS int4 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION tle_norad_id(tle) IS 'NORAD catalog number'; + +CREATE FUNCTION tle_inclination(tle) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION tle_inclination(tle) IS 'Orbital inclination in degrees'; + +CREATE FUNCTION tle_eccentricity(tle) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION tle_eccentricity(tle) IS 'Orbital eccentricity (dimensionless)'; + +CREATE FUNCTION tle_raan(tle) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION tle_raan(tle) IS 'Right ascension of ascending node in degrees'; + +CREATE FUNCTION tle_arg_perigee(tle) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION tle_arg_perigee(tle) IS 'Argument of perigee in degrees'; + +CREATE FUNCTION tle_mean_anomaly(tle) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION tle_mean_anomaly(tle) IS 'Mean anomaly in degrees'; + +CREATE FUNCTION tle_mean_motion(tle) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION tle_mean_motion(tle) IS 'Mean motion in revolutions per day'; + +CREATE FUNCTION tle_bstar(tle) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION tle_bstar(tle) IS 'B* drag coefficient (1/earth-radii)'; + +CREATE FUNCTION tle_period(tle) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION tle_period(tle) IS 'Orbital period in minutes'; + +CREATE FUNCTION tle_age(tle, timestamptz) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION tle_age(tle, timestamptz) IS 'TLE age in days (positive = past epoch, negative = before epoch)'; + +CREATE FUNCTION tle_perigee(tle) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION tle_perigee(tle) IS 'Perigee altitude in km above WGS-72 ellipsoid'; + +CREATE FUNCTION tle_apogee(tle) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION tle_apogee(tle) IS 'Apogee altitude in km above WGS-72 ellipsoid'; + +CREATE FUNCTION tle_intl_desig(tle) RETURNS text + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION tle_intl_desig(tle) IS 'International designator (COSPAR ID)'; + +CREATE FUNCTION tle_from_lines(text, text) RETURNS tle + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION tle_from_lines(text, text) IS + 'Construct TLE from separate line1/line2 text columns'; + + +-- ============================================================ +-- ECI position type: True Equator Mean Equinox (TEME) frame +-- ============================================================ + +CREATE FUNCTION eci_in(cstring) RETURNS eci_position + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION eci_out(eci_position) RETURNS cstring + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION eci_recv(internal) RETURNS eci_position + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION eci_send(eci_position) RETURNS bytea + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE TYPE eci_position ( + INPUT = eci_in, + OUTPUT = eci_out, + RECEIVE = eci_recv, + SEND = eci_send, + INTERNALLENGTH = 48, + ALIGNMENT = double, + STORAGE = plain +); + +COMMENT ON TYPE eci_position IS 'Earth-Centered Inertial position and velocity in TEME frame (km, km/s)'; + +-- ECI accessor functions + +CREATE FUNCTION eci_x(eci_position) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION eci_y(eci_position) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION eci_z(eci_position) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION eci_vx(eci_position) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION eci_vy(eci_position) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION eci_vz(eci_position) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION eci_speed(eci_position) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION eci_speed(eci_position) IS 'Velocity magnitude in km/s'; + +CREATE FUNCTION eci_altitude(eci_position) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION eci_altitude(eci_position) IS 'Approximate geocentric altitude in km (radius - WGS72_AE)'; + + +-- ============================================================ +-- Geodetic type: WGS-84 latitude/longitude/altitude +-- ============================================================ + +CREATE FUNCTION geodetic_in(cstring) RETURNS geodetic + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION geodetic_out(geodetic) RETURNS cstring + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION geodetic_recv(internal) RETURNS geodetic + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION geodetic_send(geodetic) RETURNS bytea + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE TYPE geodetic ( + INPUT = geodetic_in, + OUTPUT = geodetic_out, + RECEIVE = geodetic_recv, + SEND = geodetic_send, + INTERNALLENGTH = 24, + ALIGNMENT = double, + STORAGE = plain +); + +COMMENT ON TYPE geodetic IS 'Geodetic coordinates on WGS-84 ellipsoid (lat/lon in degrees, altitude in km)'; + +CREATE FUNCTION geodetic_lat(geodetic) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION geodetic_lon(geodetic) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION geodetic_alt(geodetic) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + + +-- ============================================================ +-- Topocentric type: observer-relative az/el/range +-- ============================================================ + +CREATE FUNCTION topocentric_in(cstring) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION topocentric_out(topocentric) RETURNS cstring + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION topocentric_recv(internal) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION topocentric_send(topocentric) RETURNS bytea + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE TYPE topocentric ( + INPUT = topocentric_in, + OUTPUT = topocentric_out, + RECEIVE = topocentric_recv, + SEND = topocentric_send, + INTERNALLENGTH = 32, + ALIGNMENT = double, + STORAGE = plain +); + +COMMENT ON TYPE topocentric IS 'Topocentric coordinates relative to observer (azimuth, elevation, range, range rate)'; + +CREATE FUNCTION topo_azimuth(topocentric) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION topo_azimuth(topocentric) IS 'Azimuth in degrees (0=N, 90=E, 180=S, 270=W)'; + +CREATE FUNCTION topo_elevation(topocentric) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION topo_elevation(topocentric) IS 'Elevation in degrees (0=horizon, 90=zenith)'; + +CREATE FUNCTION topo_range(topocentric) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION topo_range(topocentric) IS 'Slant range in km'; + +CREATE FUNCTION topo_range_rate(topocentric) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION topo_range_rate(topocentric) IS 'Range rate in km/s (positive = receding)'; + + +-- ============================================================ +-- Observer type: ground station location +-- ============================================================ + +CREATE FUNCTION observer_in(cstring) RETURNS observer + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION observer_out(observer) RETURNS cstring + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION observer_recv(internal) RETURNS observer + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION observer_send(observer) RETURNS bytea + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE TYPE observer ( + INPUT = observer_in, + OUTPUT = observer_out, + RECEIVE = observer_recv, + SEND = observer_send, + INTERNALLENGTH = 24, + ALIGNMENT = double, + STORAGE = plain +); + +COMMENT ON TYPE observer IS 'Ground observer location (accepts: 40.0N 105.3W 1655m or decimal degrees)'; + +CREATE FUNCTION observer_lat(observer) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION observer_lat(observer) IS 'Latitude in degrees (positive = North)'; + +CREATE FUNCTION observer_lon(observer) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION observer_lon(observer) IS 'Longitude in degrees (positive = East)'; + +CREATE FUNCTION observer_alt(observer) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION observer_alt(observer) IS 'Altitude in meters above WGS-84 ellipsoid'; + +CREATE FUNCTION observer_from_geodetic(float8, float8, float8 DEFAULT 0.0) RETURNS observer + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION observer_from_geodetic(float8, float8, float8) IS + 'Construct observer from lat (deg), lon (deg), altitude (meters). Avoids text formatting round-trips.'; + + +-- ============================================================ +-- Pass event type: satellite visibility window +-- ============================================================ + +CREATE FUNCTION pass_event_in(cstring) RETURNS pass_event + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION pass_event_out(pass_event) RETURNS cstring + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION pass_event_recv(internal) RETURNS pass_event + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION pass_event_send(pass_event) RETURNS bytea + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE TYPE pass_event ( + INPUT = pass_event_in, + OUTPUT = pass_event_out, + RECEIVE = pass_event_recv, + SEND = pass_event_send, + INTERNALLENGTH = 48, + ALIGNMENT = double, + STORAGE = plain +); + +COMMENT ON TYPE pass_event IS 'Satellite pass event (AOS/MAX/LOS times, max elevation, AOS/LOS azimuths)'; + +CREATE FUNCTION pass_aos_time(pass_event) RETURNS timestamptz + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION pass_aos_time(pass_event) IS 'Acquisition of signal time'; + +CREATE FUNCTION pass_max_el_time(pass_event) RETURNS timestamptz + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION pass_max_el_time(pass_event) IS 'Maximum elevation time'; + +CREATE FUNCTION pass_los_time(pass_event) RETURNS timestamptz + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION pass_los_time(pass_event) IS 'Loss of signal time'; + +CREATE FUNCTION pass_max_elevation(pass_event) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION pass_max_elevation(pass_event) IS 'Maximum elevation in degrees'; + +CREATE FUNCTION pass_aos_azimuth(pass_event) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION pass_aos_azimuth(pass_event) IS 'AOS azimuth in degrees (0=N)'; + +CREATE FUNCTION pass_los_azimuth(pass_event) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION pass_los_azimuth(pass_event) IS 'LOS azimuth in degrees (0=N)'; + +CREATE FUNCTION pass_duration(pass_event) RETURNS interval + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION pass_duration(pass_event) IS 'Pass duration (LOS - AOS)'; + + +-- ============================================================ +-- SGP4/SDP4 propagation functions +-- ============================================================ + +CREATE FUNCTION sgp4_propagate(tle, timestamptz) RETURNS eci_position + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION sgp4_propagate(tle, timestamptz) IS + 'Propagate TLE to a point in time using SGP4 (near-earth) or SDP4 (deep-space). Returns TEME ECI position/velocity.'; + +CREATE FUNCTION sgp4_propagate_safe(tle, timestamptz) RETURNS eci_position + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE PARALLEL SAFE; +COMMENT ON FUNCTION sgp4_propagate_safe(tle, timestamptz) IS + 'Like sgp4_propagate but returns NULL on error instead of raising an exception. For batch queries with potentially invalid TLEs.'; + +CREATE FUNCTION sgp4_propagate_series(tle, timestamptz, timestamptz, interval) + RETURNS TABLE(t timestamptz, x float8, y float8, z float8, vx float8, vy float8, vz float8) + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE + ROWS 100; +COMMENT ON FUNCTION sgp4_propagate_series(tle, timestamptz, timestamptz, interval) IS + 'Propagate TLE over a time range at regular intervals. Returns time series of TEME positions.'; + +CREATE FUNCTION tle_distance(tle, tle, timestamptz) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION tle_distance(tle, tle, timestamptz) IS + 'Euclidean distance in km between two TLEs at a reference time'; + + +-- ============================================================ +-- Coordinate transform functions +-- ============================================================ + +CREATE FUNCTION eci_to_geodetic(eci_position, timestamptz) RETURNS geodetic + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION eci_to_geodetic(eci_position, timestamptz) IS + 'Convert TEME ECI position to WGS-84 geodetic coordinates at given time'; + +CREATE FUNCTION eci_to_topocentric(eci_position, observer, timestamptz) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION eci_to_topocentric(eci_position, observer, timestamptz) IS + 'Convert TEME ECI position to topocentric (az/el/range) relative to observer'; + +CREATE FUNCTION subsatellite_point(tle, timestamptz) RETURNS geodetic + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION subsatellite_point(tle, timestamptz) IS + 'Subsatellite (nadir) point on WGS-84 ellipsoid at given time'; + +CREATE FUNCTION ground_track(tle, timestamptz, timestamptz, interval) + RETURNS TABLE(t timestamptz, lat float8, lon float8, alt float8) + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE + ROWS 100; +COMMENT ON FUNCTION ground_track(tle, timestamptz, timestamptz, interval) IS + 'Ground track as time series of subsatellite points (lat/lon in degrees, alt in km)'; + +CREATE FUNCTION observe(tle, observer, timestamptz) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION observe(tle, observer, timestamptz) IS + 'Propagate TLE and compute observer-relative look angles in one call. Returns topocentric (az/el/range/range_rate).'; + +CREATE FUNCTION observe_safe(tle, observer, timestamptz) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE PARALLEL SAFE; +COMMENT ON FUNCTION observe_safe(tle, observer, timestamptz) IS + 'Like observe() but returns NULL on propagation error. For batch queries with potentially invalid/decayed TLEs.'; + + +-- ============================================================ +-- Pass prediction functions +-- ============================================================ + +CREATE FUNCTION next_pass(tle, observer, timestamptz) RETURNS pass_event + AS 'MODULE_PATHNAME' LANGUAGE C STABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION next_pass(tle, observer, timestamptz) IS + 'Find the next satellite pass over observer (searches up to 7 days ahead)'; + +CREATE FUNCTION predict_passes(tle, observer, timestamptz, timestamptz, float8 DEFAULT 0.0) + RETURNS SETOF pass_event + AS 'MODULE_PATHNAME' LANGUAGE C STABLE STRICT PARALLEL SAFE + ROWS 10; +COMMENT ON FUNCTION predict_passes(tle, observer, timestamptz, timestamptz, float8) IS + 'Predict all satellite passes over observer in time window. Optional min_elevation in degrees.'; + +CREATE FUNCTION pass_visible(tle, observer, timestamptz, timestamptz) RETURNS boolean + AS 'MODULE_PATHNAME' LANGUAGE C STABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION pass_visible(tle, observer, timestamptz, timestamptz) IS + 'True if any pass occurs over observer in the time window'; + + +-- ============================================================ +-- GiST operator support functions +-- ============================================================ + +-- Overlap operator: do orbital keys overlap in altitude AND inclination? +CREATE FUNCTION tle_overlap(tle, tle) RETURNS boolean + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +-- Altitude distance operator (altitude-only, for KNN ordering) +CREATE FUNCTION tle_alt_distance(tle, tle) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE OPERATOR && ( + LEFTARG = tle, + RIGHTARG = tle, + FUNCTION = tle_overlap, + COMMUTATOR = &&, + RESTRICT = areasel, + JOIN = areajoinsel +); + +COMMENT ON OPERATOR && (tle, tle) IS 'Orbital key overlap (altitude band AND inclination range) — necessary condition for conjunction'; + +CREATE OPERATOR <-> ( + LEFTARG = tle, + RIGHTARG = tle, + FUNCTION = tle_alt_distance, + COMMUTATOR = <-> +); + +COMMENT ON OPERATOR <-> (tle, tle) IS 'Minimum altitude-band separation in km (0 if overlapping). Altitude-only — does not account for inclination. Use && for 2-D filtering.'; + + +-- ============================================================ +-- GiST operator class for 2-D orbital indexing (altitude + inclination) +-- ============================================================ + +-- GiST internal support functions +CREATE FUNCTION gist_tle_compress(internal) RETURNS internal + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION gist_tle_decompress(internal) RETURNS internal + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION gist_tle_consistent(internal, tle, smallint, oid, internal) RETURNS boolean + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION gist_tle_union(internal, internal) RETURNS internal + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION gist_tle_penalty(internal, internal, internal) RETURNS internal + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION gist_tle_picksplit(internal, internal) RETURNS internal + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION gist_tle_same(internal, internal, internal) RETURNS internal + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION gist_tle_distance(internal, tle, smallint, oid, internal) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE OPERATOR CLASS tle_ops + DEFAULT FOR TYPE tle USING gist AS + OPERATOR 3 && , + OPERATOR 15 <-> (tle, tle) FOR ORDER BY float_ops, + FUNCTION 1 gist_tle_consistent(internal, tle, smallint, oid, internal), + FUNCTION 2 gist_tle_union(internal, internal), + FUNCTION 3 gist_tle_compress(internal), + FUNCTION 4 gist_tle_decompress(internal), + FUNCTION 5 gist_tle_penalty(internal, internal, internal), + FUNCTION 6 gist_tle_picksplit(internal, internal), + FUNCTION 7 gist_tle_same(internal, internal, internal), + FUNCTION 8 gist_tle_distance(internal, tle, smallint, oid, internal); +-- pg_orrery 0.1.0 -> 0.2.0 migration +-- +-- Phase 1: Stars, comets, and Keplerian propagation. +-- Adds heliocentric type, star observation, and two-body propagation. + + +-- ============================================================ +-- Heliocentric type: ecliptic J2000 position in AU +-- ============================================================ + +CREATE TYPE heliocentric; + +CREATE FUNCTION heliocentric_in(cstring) RETURNS heliocentric + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION heliocentric_out(heliocentric) RETURNS cstring + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION heliocentric_recv(internal) RETURNS heliocentric + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE FUNCTION heliocentric_send(heliocentric) RETURNS bytea + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; + +CREATE TYPE heliocentric ( + INPUT = heliocentric_in, + OUTPUT = heliocentric_out, + RECEIVE = heliocentric_recv, + SEND = heliocentric_send, + INTERNALLENGTH = 24, + ALIGNMENT = double, + STORAGE = plain +); + +COMMENT ON TYPE heliocentric IS 'Heliocentric position in ecliptic J2000 frame (x, y, z in AU)'; + +CREATE FUNCTION helio_x(heliocentric) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION helio_x(heliocentric) IS 'X component in AU (ecliptic J2000, vernal equinox direction)'; + +CREATE FUNCTION helio_y(heliocentric) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION helio_y(heliocentric) IS 'Y component in AU (ecliptic J2000)'; + +CREATE FUNCTION helio_z(heliocentric) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION helio_z(heliocentric) IS 'Z component in AU (ecliptic J2000, north ecliptic pole)'; + +CREATE FUNCTION helio_distance(heliocentric) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION helio_distance(heliocentric) IS 'Heliocentric distance in AU'; + + +-- ============================================================ +-- Star observation functions +-- ============================================================ + +CREATE FUNCTION star_observe(float8, float8, observer, timestamptz) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION star_observe(float8, float8, observer, timestamptz) IS + 'Observe a star from (ra_hours J2000, dec_degrees J2000, observer, time). Returns topocentric az/el. Range is 0 (infinite distance).'; + +CREATE FUNCTION star_observe_safe(float8, float8, observer, timestamptz) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE PARALLEL SAFE; +COMMENT ON FUNCTION star_observe_safe(float8, float8, observer, timestamptz) IS + 'Like star_observe but returns NULL on invalid inputs. For batch queries over star catalogs.'; + + +-- ============================================================ +-- Keplerian propagation functions +-- ============================================================ + +CREATE FUNCTION kepler_propagate( + q_au float8, eccentricity float8, + inclination_deg float8, arg_perihelion_deg float8, + long_asc_node_deg float8, perihelion_jd float8, + t timestamptz +) RETURNS heliocentric + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION kepler_propagate(float8, float8, float8, float8, float8, float8, timestamptz) IS + 'Two-body Keplerian propagation from classical orbital elements. Returns heliocentric ecliptic J2000 position in AU. Handles elliptic, parabolic, and hyperbolic orbits.'; + + +-- ============================================================ +-- Comet observation +-- ============================================================ + +CREATE FUNCTION comet_observe( + q_au float8, eccentricity float8, + inclination_deg float8, arg_perihelion_deg float8, + long_asc_node_deg float8, perihelion_jd float8, + earth_x_au float8, earth_y_au float8, earth_z_au float8, + obs observer, t timestamptz +) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION comet_observe(float8, float8, float8, float8, float8, float8, float8, float8, float8, observer, timestamptz) IS + 'Observe a comet/asteroid from orbital elements. Requires Earth heliocentric ecliptic J2000 position (AU). Returns topocentric az/el with geocentric range in km.'; + + +-- ============================================================ +-- Phase 2: VSOP87 planets, ELP82B Moon, Sun observation +-- ============================================================ + +CREATE FUNCTION planet_heliocentric(int4, timestamptz) RETURNS heliocentric + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION planet_heliocentric(int4, timestamptz) IS + 'VSOP87 heliocentric ecliptic J2000 position (AU). Body IDs: 0=Sun, 1=Mercury, ..., 8=Neptune.'; + +CREATE FUNCTION planet_observe(int4, observer, timestamptz) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION planet_observe(int4, observer, timestamptz) IS + 'Observe a planet from (body_id 1-8, observer, time). Returns topocentric az/el with geocentric range in km.'; + +CREATE FUNCTION sun_observe(observer, timestamptz) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION sun_observe(observer, timestamptz) IS + 'Observe the Sun from (observer, time). Returns topocentric az/el with geocentric range in km.'; + +CREATE FUNCTION moon_observe(observer, timestamptz) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION moon_observe(observer, timestamptz) IS + 'Observe the Moon via ELP2000-82B from (observer, time). Returns topocentric az/el with geocentric range in km.'; + + +-- ============================================================ +-- Phase 3: Planetary moon observation +-- ============================================================ + +CREATE FUNCTION galilean_observe(int4, observer, timestamptz) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION galilean_observe(int4, observer, timestamptz) IS + 'Observe a Galilean moon of Jupiter via L1.2 theory. Body IDs: 0=Io, 1=Europa, 2=Ganymede, 3=Callisto.'; + +CREATE FUNCTION saturn_moon_observe(int4, observer, timestamptz) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION saturn_moon_observe(int4, observer, timestamptz) IS + 'Observe a Saturn moon via TASS 1.7. Body IDs: 0=Mimas, 1=Enceladus, 2=Tethys, 3=Dione, 4=Rhea, 5=Titan, 6=Iapetus, 7=Hyperion.'; + +CREATE FUNCTION uranus_moon_observe(int4, observer, timestamptz) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION uranus_moon_observe(int4, observer, timestamptz) IS + 'Observe a Uranus moon via GUST86. Body IDs: 0=Miranda, 1=Ariel, 2=Umbriel, 3=Titania, 4=Oberon.'; + +CREATE FUNCTION mars_moon_observe(int4, observer, timestamptz) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION mars_moon_observe(int4, observer, timestamptz) IS + 'Observe a Mars moon via MarsSat. Body IDs: 0=Phobos, 1=Deimos.'; + + +-- ============================================================ +-- Phase 3: Jupiter decametric radio burst prediction +-- ============================================================ + +CREATE FUNCTION io_phase_angle(timestamptz) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION io_phase_angle(timestamptz) IS + 'Io orbital phase angle in degrees [0,360). 0=superior conjunction (behind Jupiter). Standard Radio JOVE convention.'; + +CREATE FUNCTION jupiter_cml(observer, timestamptz) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION jupiter_cml(observer, timestamptz) IS + 'Jupiter Central Meridian Longitude, System III (1965.0), in degrees [0,360). Light-time corrected.'; + +CREATE FUNCTION jupiter_burst_probability(float8, float8) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION jupiter_burst_probability(float8, float8) IS + 'Estimated Jupiter decametric burst probability (0-1) from (io_phase_deg, cml_deg). Based on Carr et al. (1983) source regions A, B, C, D.'; + + +-- ============================================================ +-- Phase 4: Interplanetary transfer orbits (Lambert solver) +-- ============================================================ + +CREATE FUNCTION lambert_transfer( + dep_body_id int4, arr_body_id int4, + dep_time timestamptz, arr_time timestamptz, + OUT c3_departure float8, OUT c3_arrival float8, + OUT v_inf_departure float8, OUT v_inf_arrival float8, + OUT tof_days float8, OUT transfer_sma float8 +) RETURNS RECORD + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION lambert_transfer(int4, int4, timestamptz, timestamptz) IS + 'Solve Lambert transfer between two planets. Returns C3 (km^2/s^2), v_infinity (km/s), TOF (days), SMA (AU). Body IDs 1-8.'; + +CREATE FUNCTION lambert_c3(int4, int4, timestamptz, timestamptz) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION lambert_c3(int4, int4, timestamptz, timestamptz) IS + 'Departure C3 (km^2/s^2) for a Lambert transfer. Returns NULL if solver fails. For pork chop plots.'; +-- pg_orrery 0.2.0 -> 0.3.0 migration +-- +-- Adds optional JPL DE440/441 ephemeris functions. +-- Existing VSOP87/ELP2000-82B functions are unchanged (still IMMUTABLE). +-- New _de() functions are STABLE (depend on external DE binary file). +-- When DE is unavailable, _de() functions fall back to VSOP87 silently. + +-- ============================================================ +-- Phase 5: DE ephemeris functions (optional high-precision) +-- ============================================================ + +-- Planet observation with DE ephemeris + +CREATE FUNCTION planet_heliocentric_de(int4, timestamptz) RETURNS heliocentric + AS 'MODULE_PATHNAME' LANGUAGE C STABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION planet_heliocentric_de(int4, timestamptz) IS + 'Heliocentric ecliptic J2000 position via JPL DE (sub-arcsecond). Falls back to VSOP87 if DE unavailable. Body IDs: 0=Sun, 1-8=Mercury-Neptune.'; + +CREATE FUNCTION planet_observe_de(int4, observer, timestamptz) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C STABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION planet_observe_de(int4, observer, timestamptz) IS + 'Observe planet via JPL DE. Falls back to VSOP87. Body IDs: 1-8 (Mercury-Neptune).'; + +CREATE FUNCTION sun_observe_de(observer, timestamptz) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C STABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION sun_observe_de(observer, timestamptz) IS + 'Observe Sun via JPL DE. Falls back to VSOP87.'; + +CREATE FUNCTION moon_observe_de(observer, timestamptz) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C STABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION moon_observe_de(observer, timestamptz) IS + 'Observe Moon via JPL DE. Falls back to ELP2000-82B.'; + + +-- Lambert transfer with DE positions + +CREATE FUNCTION lambert_transfer_de( + dep_body_id int4, arr_body_id int4, + dep_time timestamptz, arr_time timestamptz, + OUT c3_departure float8, OUT c3_arrival float8, + OUT v_inf_departure float8, OUT v_inf_arrival float8, + OUT tof_days float8, OUT transfer_sma float8 +) RETURNS RECORD + AS 'MODULE_PATHNAME' LANGUAGE C STABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION lambert_transfer_de(int4, int4, timestamptz, timestamptz) IS + 'Lambert transfer via JPL DE positions. Falls back to VSOP87. Body IDs 1-8.'; + +CREATE FUNCTION lambert_c3_de(int4, int4, timestamptz, timestamptz) RETURNS float8 + AS 'MODULE_PATHNAME' LANGUAGE C STABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION lambert_c3_de(int4, int4, timestamptz, timestamptz) IS + 'Departure C3 via JPL DE. Falls back to VSOP87. For pork chop plots.'; + + +-- Planetary moon observation with DE parent positions + +CREATE FUNCTION galilean_observe_de(int4, observer, timestamptz) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C STABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION galilean_observe_de(int4, observer, timestamptz) IS + 'Observe Galilean moon with JPL DE parent position. L1.2 moon offsets. Body IDs: 0-3 (Io-Callisto).'; + +CREATE FUNCTION saturn_moon_observe_de(int4, observer, timestamptz) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C STABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION saturn_moon_observe_de(int4, observer, timestamptz) IS + 'Observe Saturn moon with JPL DE parent position. TASS 1.7 moon offsets. Body IDs: 0-7 (Mimas-Hyperion).'; + +CREATE FUNCTION uranus_moon_observe_de(int4, observer, timestamptz) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C STABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION uranus_moon_observe_de(int4, observer, timestamptz) IS + 'Observe Uranus moon with JPL DE parent position. GUST86 moon offsets. Body IDs: 0-4 (Miranda-Oberon).'; + +CREATE FUNCTION mars_moon_observe_de(int4, observer, timestamptz) RETURNS topocentric + AS 'MODULE_PATHNAME' LANGUAGE C STABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION mars_moon_observe_de(int4, observer, timestamptz) IS + 'Observe Mars moon with JPL DE parent position. MarsSat moon offsets. Body IDs: 0-1 (Phobos-Deimos).'; + + +-- Diagnostic function + +CREATE FUNCTION pg_orrery_ephemeris_info( + OUT provider text, OUT file_path text, + OUT start_jd float8, OUT end_jd float8, + OUT version int4, OUT au_km float8 +) RETURNS RECORD + AS 'MODULE_PATHNAME' LANGUAGE C STABLE PARALLEL SAFE; +COMMENT ON FUNCTION pg_orrery_ephemeris_info() IS + 'Returns current ephemeris provider status: VSOP87 or JPL_DE with file path, JD range, version, and AU value.'; +-- pg_orrery 0.3.0 -> 0.4.0 migration +-- +-- Adds observation-to-TLE fitting via batch weighted least-squares +-- differential correction (Vallado & Crawford 2008, AIAA 2008-6770). +-- Uses equinoctial elements internally for singularity-free optimization. +-- LAPACK dgelss_() for SVD solve. + +-- ============================================================ +-- Phase 6: Orbit determination (TLE fitting from observations) +-- ============================================================ + +-- Fit TLE from ECI position/velocity ephemeris + +CREATE FUNCTION tle_from_eci( + positions eci_position[], + times timestamptz[], + seed tle DEFAULT NULL, + fit_bstar boolean DEFAULT false, + max_iter int4 DEFAULT 15, + OUT fitted_tle tle, + OUT iterations int4, + OUT rms_final float8, + OUT rms_initial float8, + OUT status text +) RETURNS RECORD + AS 'MODULE_PATHNAME' LANGUAGE C STABLE PARALLEL SAFE; +COMMENT ON FUNCTION tle_from_eci(eci_position[], timestamptz[], tle, boolean, int4) IS + 'Fit a TLE from ECI position/velocity observations via differential correction. Returns fitted TLE, iteration count, RMS residuals, and convergence status. Requires >= 6 observations.'; + +-- Fit TLE from topocentric observations (az/el/range) + +CREATE FUNCTION tle_from_topocentric( + observations topocentric[], + times timestamptz[], + obs observer, + seed tle DEFAULT NULL, + fit_bstar boolean DEFAULT false, + max_iter int4 DEFAULT 15, + OUT fitted_tle tle, + OUT iterations int4, + OUT rms_final float8, + OUT rms_initial float8, + OUT status text +) RETURNS RECORD + AS 'MODULE_PATHNAME' LANGUAGE C STABLE PARALLEL SAFE; +COMMENT ON FUNCTION tle_from_topocentric(topocentric[], timestamptz[], observer, tle, boolean, int4) IS + 'Fit a TLE from topocentric (az/el/range) observations via differential correction. Requires seed TLE and >= 6 observations.'; + +-- Per-observation residuals diagnostic + +CREATE FUNCTION tle_fit_residuals( + fitted tle, + positions eci_position[], + times timestamptz[] +) RETURNS TABLE ( + t timestamptz, + dx_km float8, + dy_km float8, + dz_km float8, + pos_err_km float8 +) + AS 'MODULE_PATHNAME' LANGUAGE C IMMUTABLE STRICT PARALLEL SAFE; +COMMENT ON FUNCTION tle_fit_residuals(tle, eci_position[], timestamptz[]) IS + 'Compute per-observation position residuals (km) between a TLE and ECI observations. Useful for fit quality assessment.'; diff --git a/src/od_funcs.c b/src/od_funcs.c new file mode 100644 index 0000000..0e58019 --- /dev/null +++ b/src/od_funcs.c @@ -0,0 +1,483 @@ +/* + * od_funcs.c -- SQL bindings for TLE fitting functions + * + * Exposes od_solver.c to PostgreSQL as SQL-callable functions: + * tle_from_eci() -- fit TLE from ECI ephemeris + * tle_from_topocentric() -- fit TLE from topocentric observations + * tle_fit_residuals() -- per-observation residuals diagnostic + * + * Placeholder: full implementation in Commit 3. + */ + +#include "postgres.h" +#include "fmgr.h" +#include "funcapi.h" +#include "utils/timestamp.h" +#include "utils/array.h" +#include "utils/builtins.h" +#include "catalog/pg_type.h" +#include "norad.h" +#include "types.h" +#include "od_solver.h" +#include "od_math.h" + +#include +#include + +PG_FUNCTION_INFO_V1(tle_from_eci); +PG_FUNCTION_INFO_V1(tle_from_topocentric); +PG_FUNCTION_INFO_V1(tle_fit_residuals); + +/* ================================================================ + * Helper: pg_tle ↔ tle_t conversion (local copy, avoids symbol coupling) + * ================================================================ + */ + +static void +pg_tle_to_sat_code_od(const pg_tle *src, tle_t *dst) +{ + memset(dst, 0, sizeof(tle_t)); + + dst->epoch = src->epoch; + dst->xincl = src->inclination; + dst->xnodeo = src->raan; + dst->eo = src->eccentricity; + dst->omegao = src->arg_perigee; + dst->xmo = src->mean_anomaly; + dst->xno = src->mean_motion; + dst->xndt2o = src->mean_motion_dot; + dst->xndd6o = src->mean_motion_ddot; + dst->bstar = src->bstar; + + dst->norad_number = src->norad_id; + dst->bulletin_number = src->elset_num; + dst->revolution_number = src->rev_num; + dst->classification = src->classification; + dst->ephemeris_type = src->ephemeris_type; + + memcpy(dst->intl_desig, src->intl_desig, 9); +} + +static void +sat_code_to_pg_tle(const tle_t *src, pg_tle *dst) +{ + memset(dst, 0, sizeof(pg_tle)); + + dst->epoch = src->epoch; + dst->inclination = src->xincl; + dst->raan = src->xnodeo; + dst->eccentricity = src->eo; + dst->arg_perigee = src->omegao; + dst->mean_anomaly = src->xmo; + dst->mean_motion = src->xno; + dst->mean_motion_dot = src->xndt2o; + dst->mean_motion_ddot = src->xndd6o; + dst->bstar = src->bstar; + + dst->norad_id = src->norad_number; + dst->elset_num = src->bulletin_number; + dst->rev_num = src->revolution_number; + dst->classification = src->classification; + dst->ephemeris_type = src->ephemeris_type; + + memcpy(dst->intl_desig, src->intl_desig, 9); +} + + +/* ================================================================ + * tle_from_eci(eci_position[], timestamptz[], tle, boolean, int4) + * -> RECORD (fitted_tle tle, iterations int4, rms_final float8, + * rms_initial float8, status text) + * + * Fit a TLE from an array of ECI position/velocity observations. + * ================================================================ + */ +Datum +tle_from_eci(PG_FUNCTION_ARGS) +{ + ArrayType *pos_arr = PG_GETARG_ARRAYTYPE_P(0); + ArrayType *time_arr = PG_GETARG_ARRAYTYPE_P(1); + bool has_seed = !PG_ARGISNULL(2); + pg_tle *seed_pg = has_seed ? (pg_tle *) PG_GETARG_POINTER(2) : NULL; + bool fit_bstar = PG_ARGISNULL(3) ? false : PG_GETARG_BOOL(3); + int32 max_iter = PG_ARGISNULL(4) ? 15 : PG_GETARG_INT32(4); + + int n_pos, n_times; + Datum *pos_datums, *time_datums; + bool *pos_nulls, *time_nulls; + od_observation_t *obs; + od_config_t config; + od_result_t result; + tle_t seed_sat; + int i, rc; + + TupleDesc tupdesc; + Datum values[5]; + bool nulls[5]; + HeapTuple tuple; + + /* Deconstruct arrays. + * pg_eci is 48 bytes = pass-by-reference (byval=false). + * timestamptz is int64 = pass-by-value on 64-bit. */ + deconstruct_array(pos_arr, pos_arr->elemtype, sizeof(pg_eci), false, + TYPALIGN_DOUBLE, &pos_datums, &pos_nulls, &n_pos); + deconstruct_array(time_arr, TIMESTAMPTZOID, sizeof(int64), FLOAT8PASSBYVAL, + TYPALIGN_DOUBLE, &time_datums, &time_nulls, &n_times); + + if (n_pos != n_times) + ereport(ERROR, + (errcode(ERRCODE_ARRAY_SUBSCRIPT_ERROR), + errmsg("positions and times arrays must have same length: " + "%d vs %d", n_pos, n_times))); + + if (n_pos < 6) + ereport(ERROR, + (errcode(ERRCODE_INVALID_PARAMETER_VALUE), + errmsg("at least 6 observations required, got %d", n_pos))); + + /* Build observation array */ + obs = (od_observation_t *) palloc(sizeof(od_observation_t) * n_pos); + + for (i = 0; i < n_pos; i++) + { + pg_eci *eci = (pg_eci *) DatumGetPointer(pos_datums[i]); + int64 ts = DatumGetInt64(time_datums[i]); + double jd = PG_EPOCH_JD + ((double)ts / (double)USECS_PER_DAY); + + obs[i].jd = jd; + obs[i].data[0] = eci->x; + obs[i].data[1] = eci->y; + obs[i].data[2] = eci->z; + obs[i].data[3] = eci->vx; /* already km/s */ + obs[i].data[4] = eci->vy; + obs[i].data[5] = eci->vz; + } + + /* Configure solver */ + memset(&config, 0, sizeof(config)); + config.obs_type = OD_OBS_ECI; + config.fit_bstar = fit_bstar ? 1 : 0; + config.max_iter = max_iter; + config.observer = NULL; + + /* Convert seed TLE if provided */ + if (has_seed) + pg_tle_to_sat_code_od(seed_pg, &seed_sat); + + memset(&result, 0, sizeof(result)); + + /* Run solver */ + rc = od_fit_tle(obs, n_pos, has_seed ? &seed_sat : NULL, &config, &result); + + pfree(obs); + + if (rc != 0) + ereport(ERROR, + (errcode(ERRCODE_DATA_EXCEPTION), + errmsg("TLE fitting failed: %s", result.status))); + + /* Build result tuple */ + if (get_call_result_type(fcinfo, NULL, &tupdesc) != TYPEFUNC_COMPOSITE) + ereport(ERROR, + (errcode(ERRCODE_FEATURE_NOT_SUPPORTED), + errmsg("function returning record called in context " + "that cannot accept type record"))); + tupdesc = BlessTupleDesc(tupdesc); + + memset(nulls, 0, sizeof(nulls)); + + { + pg_tle *fitted = (pg_tle *) palloc(sizeof(pg_tle)); + sat_code_to_pg_tle(&result.fitted_tle, fitted); + values[0] = PointerGetDatum(fitted); + } + values[1] = Int32GetDatum(result.iterations); + values[2] = Float8GetDatum(result.rms_final); + values[3] = Float8GetDatum(result.rms_initial); + values[4] = CStringGetTextDatum(result.status); + + tuple = heap_form_tuple(tupdesc, values, nulls); + PG_RETURN_DATUM(HeapTupleGetDatum(tuple)); +} + + +/* ================================================================ + * tle_from_topocentric(topocentric[], timestamptz[], observer, + * tle, boolean, int4) + * -> RECORD (same as tle_from_eci) + * ================================================================ + */ +Datum +tle_from_topocentric(PG_FUNCTION_ARGS) +{ + ArrayType *topo_arr = PG_GETARG_ARRAYTYPE_P(0); + ArrayType *time_arr = PG_GETARG_ARRAYTYPE_P(1); + pg_observer *obs_pg = (pg_observer *) PG_GETARG_POINTER(2); + bool has_seed = !PG_ARGISNULL(3); + pg_tle *seed_pg = has_seed ? (pg_tle *) PG_GETARG_POINTER(3) : NULL; + bool fit_bstar = PG_ARGISNULL(4) ? false : PG_GETARG_BOOL(4); + int32 max_iter = PG_ARGISNULL(5) ? 15 : PG_GETARG_INT32(5); + + int n_topo, n_times; + od_observation_t *obs; + od_config_t config; + od_observer_t observer; + od_result_t result; + tle_t seed_sat; + int i, rc; + + TupleDesc tupdesc; + Datum values[5]; + bool nulls[5]; + HeapTuple tuple; + + /* Build observer */ + observer.lat = obs_pg->lat; + observer.lon = obs_pg->lon; + observer.alt_m = obs_pg->alt_m; + od_observer_to_ecef(observer.lat, observer.lon, observer.alt_m, + observer.ecef); + + /* Deconstruct arrays */ + { + Datum *topo_datums; + bool *topo_nulls; + Datum *time_datums; + bool *time_nulls; + + deconstruct_array(topo_arr, topo_arr->elemtype, sizeof(pg_topocentric), + false, TYPALIGN_DOUBLE, + &topo_datums, &topo_nulls, &n_topo); + deconstruct_array(time_arr, TIMESTAMPTZOID, sizeof(int64), FLOAT8PASSBYVAL, + TYPALIGN_DOUBLE, &time_datums, &time_nulls, &n_times); + + if (n_topo != n_times) + ereport(ERROR, + (errcode(ERRCODE_ARRAY_SUBSCRIPT_ERROR), + errmsg("observations and times arrays must have same length: " + "%d vs %d", n_topo, n_times))); + + if (n_topo < 6) + ereport(ERROR, + (errcode(ERRCODE_INVALID_PARAMETER_VALUE), + errmsg("at least 6 observations required, got %d", n_topo))); + + obs = (od_observation_t *) palloc(sizeof(od_observation_t) * n_topo); + + for (i = 0; i < n_topo; i++) + { + pg_topocentric *topo = (pg_topocentric *) DatumGetPointer(topo_datums[i]); + int64 ts = DatumGetInt64(time_datums[i]); + double jd = PG_EPOCH_JD + ((double)ts / (double)USECS_PER_DAY); + + obs[i].jd = jd; + obs[i].data[0] = topo->azimuth; + obs[i].data[1] = topo->elevation; + obs[i].data[2] = topo->range_km; + } + } + + /* Configure solver */ + memset(&config, 0, sizeof(config)); + config.obs_type = OD_OBS_TOPO; + config.fit_bstar = fit_bstar ? 1 : 0; + config.max_iter = max_iter; + config.observer = &observer; + + /* Seed TLE required for topocentric */ + if (!has_seed) + ereport(ERROR, + (errcode(ERRCODE_INVALID_PARAMETER_VALUE), + errmsg("seed TLE required for topocentric fitting"))); + + pg_tle_to_sat_code_od(seed_pg, &seed_sat); + + memset(&result, 0, sizeof(result)); + + rc = od_fit_tle(obs, n_topo, &seed_sat, &config, &result); + + pfree(obs); + + if (rc != 0) + ereport(ERROR, + (errcode(ERRCODE_DATA_EXCEPTION), + errmsg("TLE fitting failed: %s", result.status))); + + /* Build result tuple */ + if (get_call_result_type(fcinfo, NULL, &tupdesc) != TYPEFUNC_COMPOSITE) + ereport(ERROR, + (errcode(ERRCODE_FEATURE_NOT_SUPPORTED), + errmsg("function returning record called in context " + "that cannot accept type record"))); + tupdesc = BlessTupleDesc(tupdesc); + + memset(nulls, 0, sizeof(nulls)); + + { + pg_tle *fitted = (pg_tle *) palloc(sizeof(pg_tle)); + sat_code_to_pg_tle(&result.fitted_tle, fitted); + values[0] = PointerGetDatum(fitted); + } + values[1] = Int32GetDatum(result.iterations); + values[2] = Float8GetDatum(result.rms_final); + values[3] = Float8GetDatum(result.rms_initial); + values[4] = CStringGetTextDatum(result.status); + + tuple = heap_form_tuple(tupdesc, values, nulls); + PG_RETURN_DATUM(HeapTupleGetDatum(tuple)); +} + + +/* ================================================================ + * tle_fit_residuals(tle, eci_position[], timestamptz[]) + * -> TABLE (t timestamptz, dx_km float8, dy_km float8, dz_km float8, + * pos_err_km float8) + * + * Compute per-observation residuals between a TLE and ECI observations. + * ================================================================ + */ +Datum +tle_fit_residuals(PG_FUNCTION_ARGS) +{ + FuncCallContext *funcctx; + + if (SRF_IS_FIRSTCALL()) + { + MemoryContext oldctx; + pg_tle *tle; + ArrayType *pos_arr; + ArrayType *time_arr; + int n_pos, n_times; + Datum *pos_datums; + bool *pos_nulls; + Datum *time_datums; + bool *time_nulls; + TupleDesc tupdesc; + + /* Context for residual data (persists across calls) */ + typedef struct { + tle_t sat; + double *params; + int is_deep; + int n_obs; + double *jds; /* Julian dates */ + double *obs_pos; /* observed positions (3 * n_obs) */ + } residual_ctx; + + residual_ctx *ctx; + int i; + + funcctx = SRF_FIRSTCALL_INIT(); + oldctx = MemoryContextSwitchTo(funcctx->multi_call_memory_ctx); + + tle = (pg_tle *) PG_GETARG_POINTER(0); + pos_arr = PG_GETARG_ARRAYTYPE_P(1); + time_arr = PG_GETARG_ARRAYTYPE_P(2); + + deconstruct_array(pos_arr, pos_arr->elemtype, sizeof(pg_eci), false, + TYPALIGN_DOUBLE, &pos_datums, &pos_nulls, &n_pos); + deconstruct_array(time_arr, TIMESTAMPTZOID, sizeof(int64), FLOAT8PASSBYVAL, + TYPALIGN_DOUBLE, &time_datums, &time_nulls, &n_times); + + if (n_pos != n_times) + ereport(ERROR, + (errcode(ERRCODE_ARRAY_SUBSCRIPT_ERROR), + errmsg("positions and times arrays must have same length"))); + + ctx = (residual_ctx *) palloc0(sizeof(residual_ctx)); + pg_tle_to_sat_code_od(tle, &ctx->sat); + + ctx->is_deep = select_ephemeris(&ctx->sat); + if (ctx->is_deep < 0) + ereport(ERROR, + (errcode(ERRCODE_DATA_EXCEPTION), + errmsg("invalid TLE for residual computation"))); + + ctx->params = (double *) palloc(sizeof(double) * N_SAT_PARAMS); + if (ctx->is_deep) + SDP4_init(ctx->params, &ctx->sat); + else + SGP4_init(ctx->params, &ctx->sat); + + ctx->n_obs = n_pos; + ctx->jds = (double *) palloc(sizeof(double) * n_pos); + ctx->obs_pos = (double *) palloc(sizeof(double) * 3 * n_pos); + + for (i = 0; i < n_pos; i++) + { + pg_eci *eci = (pg_eci *) DatumGetPointer(pos_datums[i]); + int64 ts = DatumGetInt64(time_datums[i]); + + ctx->jds[i] = PG_EPOCH_JD + ((double)ts / (double)USECS_PER_DAY); + ctx->obs_pos[i * 3 + 0] = eci->x; + ctx->obs_pos[i * 3 + 1] = eci->y; + ctx->obs_pos[i * 3 + 2] = eci->z; + } + + funcctx->max_calls = n_pos; + funcctx->user_fctx = ctx; + + tupdesc = CreateTemplateTupleDesc(5); + TupleDescInitEntry(tupdesc, 1, "t", TIMESTAMPTZOID, -1, 0); + TupleDescInitEntry(tupdesc, 2, "dx_km", FLOAT8OID, -1, 0); + TupleDescInitEntry(tupdesc, 3, "dy_km", FLOAT8OID, -1, 0); + TupleDescInitEntry(tupdesc, 4, "dz_km", FLOAT8OID, -1, 0); + TupleDescInitEntry(tupdesc, 5, "pos_err_km", FLOAT8OID, -1, 0); + + funcctx->tuple_desc = BlessTupleDesc(tupdesc); + + MemoryContextSwitchTo(oldctx); + } + + funcctx = SRF_PERCALL_SETUP(); + + if (funcctx->call_cntr < funcctx->max_calls) + { + typedef struct { + tle_t sat; + double *params; + int is_deep; + int n_obs; + double *jds; + double *obs_pos; + } residual_ctx; + + residual_ctx *ctx = (residual_ctx *) funcctx->user_fctx; + int idx = funcctx->call_cntr; + double tsince, pos[3], vel[3]; + double dx, dy, dz, pos_err; + int err; + int64 ts; + Datum values[5]; + bool nulls[5]; + HeapTuple tuple; + + tsince = (ctx->jds[idx] - ctx->sat.epoch) * 1440.0; + + if (ctx->is_deep) + err = SDP4(tsince, &ctx->sat, ctx->params, pos, vel); + else + err = SGP4(tsince, &ctx->sat, ctx->params, pos, vel); + + dx = ctx->obs_pos[idx * 3 + 0] - pos[0]; + dy = ctx->obs_pos[idx * 3 + 1] - pos[1]; + dz = ctx->obs_pos[idx * 3 + 2] - pos[2]; + pos_err = sqrt(dx * dx + dy * dy + dz * dz); + + ts = (int64)((ctx->jds[idx] - PG_EPOCH_JD) * (double)USECS_PER_DAY); + + memset(nulls, 0, sizeof(nulls)); + values[0] = Int64GetDatum(ts); + values[1] = Float8GetDatum(dx); + values[2] = Float8GetDatum(dy); + values[3] = Float8GetDatum(dz); + values[4] = Float8GetDatum(pos_err); + + (void)err; + + tuple = heap_form_tuple(funcctx->tuple_desc, values, nulls); + SRF_RETURN_NEXT(funcctx, HeapTupleGetDatum(tuple)); + } + + SRF_RETURN_DONE(funcctx); +} diff --git a/src/od_math.c b/src/od_math.c new file mode 100644 index 0000000..aadc0b1 --- /dev/null +++ b/src/od_math.c @@ -0,0 +1,513 @@ +/* + * od_math.c -- Orbital determination math implementation + * + * Element conversions and inverse coordinate transforms for the + * TLE fitting pipeline. All orbital mechanics use WGS-72 constants + * to maintain chain of custody with SGP4/SDP4. + * + * References: + * Vallado & Crawford, "SGP4 Orbit Determination", AIAA 2008-6770 + * Vallado, "Fundamentals of Astrodynamics and Applications", 4th ed. + * Hoots & Roehrich, "Spacetrack Report No. 3", 1980 + */ + +#include "od_math.h" + +/* WGS-72 gravitational parameter — must match SGP4's internal value. + * 398600.8 km^3/s^2 (from STR#3), NOT the WGS-84 value 398600.4418. */ +#define MU_KM3_S2 398600.8 + +/* WGS-72 equatorial radius in km */ +#define RE_KM 6378.135 + +/* SGP4 internal constants from norad_in.h */ +#define XKE 0.0743669161331734132 +#define CK2 (0.5 * 1.082616e-3) +#define TWO_THIRDS (2.0 / 3.0) + +/* WGS-84 constants (coordinate transforms only) */ +#define WGS84_A_KM 6378.137 +#define WGS84_F (1.0 / 298.257223563) +#define WGS84_E2 (WGS84_F * (2.0 - WGS84_F)) + +/* Earth rotation rate, rad/s */ +#define OMEGA_EARTH 7.2921158553e-5 + + +/* ================================================================ + * GMST and observer position (duplicated to avoid symbol coupling) + * ================================================================ + */ + +double +od_gmst_from_jd(double jd) +{ + double t_ut1 = (jd - 2451545.0) / 36525.0; + double gmst = 67310.54841 + + (876600.0 * 3600.0 + 8640184.812866) * t_ut1 + + 0.093104 * t_ut1 * t_ut1 + - 6.2e-6 * t_ut1 * t_ut1 * t_ut1; + + gmst = fmod(gmst * M_PI / 43200.0, 2.0 * M_PI); + if (gmst < 0.0) + gmst += 2.0 * M_PI; + return gmst; +} + +void +od_observer_to_ecef(double lat, double lon, double alt_m, + double pos_ecef[3]) +{ + double sin_lat = sin(lat); + double cos_lat = cos(lat); + double N = WGS84_A_KM / sqrt(1.0 - WGS84_E2 * sin_lat * sin_lat); + double alt_km = alt_m / 1000.0; + + pos_ecef[0] = (N + alt_km) * cos_lat * cos(lon); + pos_ecef[1] = (N + alt_km) * cos_lat * sin(lon); + pos_ecef[2] = (N * (1.0 - WGS84_E2) + alt_km) * sin_lat; +} + + +/* ================================================================ + * Inverse coordinate transforms + * ================================================================ + */ + +/* + * ECEF → TEME: inverse of teme_to_ecef (coord_funcs.c:84-105). + * Rotate position by +GMST (instead of -GMST), subtract Earth + * rotation cross-product from velocity. + */ +void +od_ecef_to_teme(const double pos_ecef[3], const double vel_ecef[3], + double gmst, + double pos_teme[3], double vel_teme[3]) +{ + double cos_g = cos(gmst); + double sin_g = sin(gmst); + + /* Position: rotate by +GMST (inverse of -GMST rotation) */ + pos_teme[0] = cos_g * pos_ecef[0] - sin_g * pos_ecef[1]; + pos_teme[1] = sin_g * pos_ecef[0] + cos_g * pos_ecef[1]; + pos_teme[2] = pos_ecef[2]; + + if (vel_teme && vel_ecef) + { + /* + * Forward: vel_ecef = R(-g)*vel_teme + omega x pos_ecef + * Inverse: vel_teme = R(+g)*(vel_ecef - omega x pos_ecef) + * + * omega x pos_ecef = (-omega*y, +omega*x, 0) in ECEF + * but the forward code adds omega*pos_ecef[1] to vel_ecef[0] + * and subtracts omega*pos_ecef[0] from vel_ecef[1], so undo that. + */ + double vel_inertial_ecef_x = vel_ecef[0] - OMEGA_EARTH * pos_ecef[1]; + double vel_inertial_ecef_y = vel_ecef[1] + OMEGA_EARTH * pos_ecef[0]; + + vel_teme[0] = cos_g * vel_inertial_ecef_x - sin_g * vel_inertial_ecef_y; + vel_teme[1] = sin_g * vel_inertial_ecef_x + cos_g * vel_inertial_ecef_y; + vel_teme[2] = vel_ecef[2]; + } +} + +/* + * Topocentric (az, el, range) → ECEF satellite position. + * Inverse of ecef_to_topocentric (coord_funcs.c:165-190). + * + * The forward transform computes SEU (South, East, Up) components + * from the ECEF range vector, then derives az/el. We invert: + * 1. Recover SEU from az/el/range + * 2. Transpose the SEU rotation matrix (orthogonal → inverse = transpose) + * 3. Add observer ECEF position + */ +void +od_topocentric_to_ecef(double az, double el, double range_km, + const double obs_ecef[3], + double obs_lat, double obs_lon, + double sat_ecef[3]) +{ + /* Recover SEU from az/el/range. + * Forward: az = atan2(east, -south), el = asin(up / range) + * So: south = -range*cos(el)*cos(az), east = range*cos(el)*sin(az) */ + double cos_el = cos(el); + double south = -range_km * cos_el * cos(az); + double east = range_km * cos_el * sin(az); + double up = range_km * sin(el); + + /* The forward rotation (ECEF range → SEU) uses: + * south = sin_lat*cos_lon*dx + sin_lat*sin_lon*dy - cos_lat*dz + * east = -sin_lon*dx + cos_lon*dy + * up = cos_lat*cos_lon*dx + cos_lat*sin_lon*dy + sin_lat*dz + * + * This is R * [dx, dy, dz]^T where R is orthogonal. + * Inverse: [dx, dy, dz]^T = R^T * [south, east, up]^T + */ + double sin_lat = sin(obs_lat); + double cos_lat = cos(obs_lat); + double sin_lon = sin(obs_lon); + double cos_lon = cos(obs_lon); + + double dx = sin_lat * cos_lon * south - sin_lon * east + cos_lat * cos_lon * up; + double dy = sin_lat * sin_lon * south + cos_lon * east + cos_lat * sin_lon * up; + double dz = -cos_lat * south + sin_lat * up; + + sat_ecef[0] = obs_ecef[0] + dx; + sat_ecef[1] = obs_ecef[1] + dy; + sat_ecef[2] = obs_ecef[2] + dz; +} + + +/* ================================================================ + * ECI ↔ Keplerian element conversion + * ================================================================ + */ + +/* + * ECI state vector → classical Keplerian elements. + * + * Standard r,v → orbital elements conversion (Vallado Algorithm 9). + * Uses WGS-72 mu for SGP4 consistency. + * + * Returns 0 on success, -1 if orbit is degenerate (r ≈ 0). + */ +int +od_eci_to_keplerian(const double pos[3], const double vel[3], + od_keplerian_t *kep) +{ + double r_mag, v_mag, rdotv; + double h[3], h_mag; + double n_vec[3], n_mag; + double e_vec[3], ecc; + double a, p; + double inc, raan, argp, nu, M, E; + + /* Position and velocity magnitudes */ + r_mag = sqrt(pos[0]*pos[0] + pos[1]*pos[1] + pos[2]*pos[2]); + v_mag = sqrt(vel[0]*vel[0] + vel[1]*vel[1] + vel[2]*vel[2]); + + if (r_mag < 1.0e-10) + return -1; + + rdotv = pos[0]*vel[0] + pos[1]*vel[1] + pos[2]*vel[2]; + + /* Angular momentum h = r x v */ + h[0] = pos[1]*vel[2] - pos[2]*vel[1]; + h[1] = pos[2]*vel[0] - pos[0]*vel[2]; + h[2] = pos[0]*vel[1] - pos[1]*vel[0]; + h_mag = sqrt(h[0]*h[0] + h[1]*h[1] + h[2]*h[2]); + + if (h_mag < 1.0e-10) + return -1; + + /* Node vector n = k x h (k = [0,0,1]) */ + n_vec[0] = -h[1]; + n_vec[1] = h[0]; + n_vec[2] = 0.0; + n_mag = sqrt(n_vec[0]*n_vec[0] + n_vec[1]*n_vec[1]); + + /* Eccentricity vector: e = (v x h)/mu - r/|r| */ + e_vec[0] = (vel[1]*h[2] - vel[2]*h[1]) / MU_KM3_S2 - pos[0] / r_mag; + e_vec[1] = (vel[2]*h[0] - vel[0]*h[2]) / MU_KM3_S2 - pos[1] / r_mag; + e_vec[2] = (vel[0]*h[1] - vel[1]*h[0]) / MU_KM3_S2 - pos[2] / r_mag; + ecc = sqrt(e_vec[0]*e_vec[0] + e_vec[1]*e_vec[1] + e_vec[2]*e_vec[2]); + + /* Semi-major axis from vis-viva */ + a = 1.0 / (2.0 / r_mag - v_mag * v_mag / MU_KM3_S2); + p = h_mag * h_mag / MU_KM3_S2; + + /* Inclination */ + inc = acos(h[2] / h_mag); + if (inc < 0.0) inc = 0.0; /* clamp rounding errors */ + + /* RAAN */ + if (n_mag > 1.0e-10) + { + raan = acos(n_vec[0] / n_mag); + if (n_vec[1] < 0.0) + raan = 2.0 * M_PI - raan; + } + else + { + raan = 0.0; /* equatorial orbit */ + } + + /* Argument of perigee */ + if (n_mag > 1.0e-10 && ecc > 1.0e-10) + { + double ndote = (n_vec[0]*e_vec[0] + n_vec[1]*e_vec[1] + + n_vec[2]*e_vec[2]) / (n_mag * ecc); + if (ndote > 1.0) ndote = 1.0; + if (ndote < -1.0) ndote = -1.0; + argp = acos(ndote); + if (e_vec[2] < 0.0) + argp = 2.0 * M_PI - argp; + } + else if (ecc > 1.0e-10) + { + /* Equatorial: measure from x-axis */ + argp = atan2(e_vec[1], e_vec[0]); + if (argp < 0.0) argp += 2.0 * M_PI; + } + else + { + argp = 0.0; /* circular */ + } + + /* True anomaly */ + if (ecc > 1.0e-10) + { + double edotr = (e_vec[0]*pos[0] + e_vec[1]*pos[1] + + e_vec[2]*pos[2]) / (ecc * r_mag); + if (edotr > 1.0) edotr = 1.0; + if (edotr < -1.0) edotr = -1.0; + nu = acos(edotr); + if (rdotv < 0.0) + nu = 2.0 * M_PI - nu; + } + else + { + /* Circular: measure from node or x-axis */ + if (n_mag > 1.0e-10) + { + double ndotr = (n_vec[0]*pos[0] + n_vec[1]*pos[1]) / (n_mag * r_mag); + if (ndotr > 1.0) ndotr = 1.0; + if (ndotr < -1.0) ndotr = -1.0; + nu = acos(ndotr); + if (pos[2] < 0.0) + nu = 2.0 * M_PI - nu; + } + else + { + nu = atan2(pos[1], pos[0]); + if (nu < 0.0) nu += 2.0 * M_PI; + } + } + + /* True anomaly → eccentric anomaly → mean anomaly */ + E = atan2(sqrt(1.0 - ecc * ecc) * sin(nu), ecc + cos(nu)); + M = E - ecc * sin(E); + if (M < 0.0) M += 2.0 * M_PI; + + /* Mean motion from semi-major axis (rad/s → rad/min) */ + kep->n = sqrt(MU_KM3_S2 / (a * a * a)) * 60.0; + kep->ecc = ecc; + kep->inc = inc; + kep->raan = od_normalize_angle(raan); + kep->argp = od_normalize_angle(argp); + kep->M = od_normalize_angle(M); + kep->bstar = 0.0; + + (void)p; /* used implicitly in derivation */ + return 0; +} + +/* + * Classical Keplerian → ECI state vector. + * + * Standard perifocal frame computation (Vallado Algorithm 10). + */ +void +od_keplerian_to_eci(const od_keplerian_t *kep, + double pos[3], double vel[3]) +{ + /* Mean motion rad/min → semi-major axis */ + double n_rad_s = kep->n / 60.0; + double a = pow(MU_KM3_S2 / (n_rad_s * n_rad_s), 1.0 / 3.0); + double ecc = kep->ecc; + double inc = kep->inc; + double raan = kep->raan; + double argp = kep->argp; + double M = kep->M; + double E, dE; + int i; + double cos_E, sin_E; + double r_peri[2], v_peri[2]; + double r_mag, v_coeff; + + /* Solve Kepler's equation: M = E - e*sin(E) */ + E = M; + for (i = 0; i < 20; i++) + { + dE = (M - E + ecc * sin(E)) / (1.0 - ecc * cos(E)); + E += dE; + if (fabs(dE) < 1.0e-14) + break; + } + + cos_E = cos(E); + sin_E = sin(E); + + /* Perifocal coordinates */ + r_peri[0] = a * (cos_E - ecc); + r_peri[1] = a * sqrt(1.0 - ecc * ecc) * sin_E; + + r_mag = a * (1.0 - ecc * cos_E); + v_coeff = sqrt(MU_KM3_S2 * a) / r_mag; + v_peri[0] = -v_coeff * sin_E; + v_peri[1] = v_coeff * sqrt(1.0 - ecc * ecc) * cos_E; + + /* Perifocal → ECI rotation */ + { + double cos_raan = cos(raan), sin_raan = sin(raan); + double cos_argp = cos(argp), sin_argp = sin(argp); + double cos_inc = cos(inc), sin_inc = sin(inc); + + /* Rotation matrix columns (P and Q vectors) */ + double Px = cos_raan * cos_argp - sin_raan * sin_argp * cos_inc; + double Py = sin_raan * cos_argp + cos_raan * sin_argp * cos_inc; + double Pz = sin_argp * sin_inc; + + double Qx = -cos_raan * sin_argp - sin_raan * cos_argp * cos_inc; + double Qy = -sin_raan * sin_argp + cos_raan * cos_argp * cos_inc; + double Qz = cos_argp * sin_inc; + + pos[0] = Px * r_peri[0] + Qx * r_peri[1]; + pos[1] = Py * r_peri[0] + Qy * r_peri[1]; + pos[2] = Pz * r_peri[0] + Qz * r_peri[1]; + + vel[0] = Px * v_peri[0] + Qx * v_peri[1]; + vel[1] = Py * v_peri[0] + Qy * v_peri[1]; + vel[2] = Pz * v_peri[0] + Qz * v_peri[1]; + } +} + + +/* ================================================================ + * Keplerian ↔ equinoctial element conversion + * ================================================================ + */ + +/* + * Keplerian → equinoctial (Vallado 2008 Eq. 1-5) + * + * af = e * cos(omega + RAAN) + * ag = e * sin(omega + RAAN) + * chi = tan(i/2) * cos(RAAN) + * psi = tan(i/2) * sin(RAAN) + * L0 = M + omega + RAAN + */ +void +od_keplerian_to_equinoctial(const od_keplerian_t *kep, + od_equinoctial_t *eq) +{ + double omega_plus_raan = kep->argp + kep->raan; + double tan_half_inc = tan(kep->inc / 2.0); + + eq->n = kep->n; + eq->af = kep->ecc * cos(omega_plus_raan); + eq->ag = kep->ecc * sin(omega_plus_raan); + eq->chi = tan_half_inc * cos(kep->raan); + eq->psi = tan_half_inc * sin(kep->raan); + eq->L0 = od_normalize_angle(kep->M + omega_plus_raan); + eq->bstar = kep->bstar; +} + +/* + * Equinoctial → Keplerian (inverse of Vallado 2008 Eq. 1-5) + * + * e = sqrt(af^2 + ag^2) + * i = 2 * atan(sqrt(chi^2 + psi^2)) + * RAAN = atan2(psi, chi) + * omega = atan2(ag, af) - RAAN + * M = L0 - omega - RAAN + */ +void +od_equinoctial_to_keplerian(const od_equinoctial_t *eq, + od_keplerian_t *kep) +{ + double ecc = sqrt(eq->af * eq->af + eq->ag * eq->ag); + double inc = 2.0 * atan(sqrt(eq->chi * eq->chi + eq->psi * eq->psi)); + double raan = atan2(eq->psi, eq->chi); + double omega_plus_raan = atan2(eq->ag, eq->af); + double argp, M; + + if (raan < 0.0) raan += 2.0 * M_PI; + + argp = omega_plus_raan - raan; + M = eq->L0 - omega_plus_raan; + + kep->n = eq->n; + kep->ecc = ecc; + kep->inc = inc; + kep->raan = od_normalize_angle(raan); + kep->argp = od_normalize_angle(argp); + kep->M = od_normalize_angle(M); + kep->bstar = eq->bstar; +} + + +/* ================================================================ + * Brouwer ↔ Kozai mean motion conversion + * ================================================================ + */ + +/* + * Forward function: Kozai n → Brouwer n. + * + * Mirrors sxpall_common_init() from common.c:36-54. + * Given n_kozai (the TLE xno field), computes the Brouwer + * mean motion xnodp that SGP4 uses internally. + */ +static double +kozai_to_brouwer_forward(double n_kozai, double ecc, double inc) +{ + double cosio = cos(inc); + double cosio2 = cosio * cosio; + double eosq = ecc * ecc; + double betao2 = 1.0 - eosq; + double betao = sqrt(betao2); + double a1, del1, ao, delo, xnodp; + double tval; + + a1 = pow(XKE / n_kozai, TWO_THIRDS); + tval = 1.5 * CK2 * (3.0 * cosio2 - 1.0) / (betao * betao2); + del1 = tval / (a1 * a1); + ao = a1 * (1.0 - del1 * (1.0 / 3.0 + del1 * (1.0 + 134.0 / 81.0 * del1))); + delo = tval / (ao * ao); + xnodp = n_kozai / (1.0 + delo); + + return xnodp; +} + +/* + * Kozai → Brouwer: direct computation (no iteration needed). + */ +double +od_kozai_to_brouwer_n(double n_kozai, double ecc, double inc) +{ + return kozai_to_brouwer_forward(n_kozai, ecc, inc); +} + +/* + * Brouwer → Kozai: Newton-Raphson inversion. + * + * Solves f(n_kozai) = n_brouwer for n_kozai, where + * f = kozai_to_brouwer_forward. Converges in 2-4 iterations. + */ +double +od_brouwer_to_kozai_n(double n_brouwer, double ecc, double inc) +{ + double n_k = n_brouwer; /* initial guess */ + double h = 1.0e-10; + int i; + + for (i = 0; i < 20; i++) + { + double f_k = kozai_to_brouwer_forward(n_k, ecc, inc); + double f_kh = kozai_to_brouwer_forward(n_k + h, ecc, inc); + double fp = (f_kh - f_k) / h; + double delta; + + if (fabs(fp) < 1.0e-20) + break; + + delta = (f_k - n_brouwer) / fp; + n_k -= delta; + + if (fabs(delta) < 1.0e-14) + break; + } + + return n_k; +} diff --git a/src/od_math.h b/src/od_math.h new file mode 100644 index 0000000..7a02dd9 --- /dev/null +++ b/src/od_math.h @@ -0,0 +1,141 @@ +/* + * od_math.h -- Orbital determination math: element converters and + * inverse coordinate transforms + * + * Implements the inverse pipeline for observation-to-TLE fitting: + * ECI ↔ Keplerian ↔ equinoctial (Vallado 2008 Eq. 1-5) + * Brouwer ↔ Kozai mean motion (inverse of sxpall_common_init) + * ECEF → TEME, topocentric → ECEF (inverse coord transforms) + * + * All propagation-side constants use WGS-72 from norad_in.h. + * Coordinate output constants use WGS-84 from types.h. + */ + +#ifndef PG_ORRERY_OD_MATH_H +#define PG_ORRERY_OD_MATH_H + +#include + +/* + * Equinoctial elements (Vallado 2008 Eq. 1-5) + * + * Singularity-free for e < 1, i < 180 deg. The DC solver + * perturbs these instead of classical elements, avoiding + * ill-conditioned Jacobians near circular or equatorial orbits. + */ +typedef struct +{ + double n; /* mean motion, rad/min */ + double af; /* e * cos(omega + RAAN) */ + double ag; /* e * sin(omega + RAAN) */ + double chi; /* tan(i/2) * cos(RAAN) */ + double psi; /* tan(i/2) * sin(RAAN) */ + double L0; /* mean longitude at epoch: M + omega + RAAN */ + double bstar; /* drag coefficient (7th state, optional) */ +} od_equinoctial_t; + +/* + * Classical Keplerian elements (osculating) + * + * Intermediate representation between ECI state vectors + * and equinoctial elements. All angles in radians. + */ +typedef struct +{ + double n; /* mean motion, rad/min */ + double ecc; /* eccentricity */ + double inc; /* inclination, radians */ + double raan; /* right ascension of ascending node, radians */ + double argp; /* argument of perigee, radians */ + double M; /* mean anomaly at epoch, radians */ + double bstar; /* drag coefficient */ +} od_keplerian_t; + +/* ── Element conversion functions ──────────────────────── */ + +/* + * ECI state vector (TEME, km, km/s) → classical Keplerian + * + * Uses WGS-72 mu (398600.8 km^3/s^2) for consistency with SGP4. + * Returns 0 on success, -1 on degenerate orbit. + */ +int od_eci_to_keplerian(const double pos[3], const double vel[3], + od_keplerian_t *kep); + +/* + * Classical Keplerian → ECI state vector (TEME, km, km/s) + * + * Inverse of od_eci_to_keplerian. Uses WGS-72 mu. + */ +void od_keplerian_to_eci(const od_keplerian_t *kep, + double pos[3], double vel[3]); + +/* + * Classical Keplerian ↔ equinoctial element conversion + * (Vallado 2008 Eq. 1-5 and their inverses) + */ +void od_keplerian_to_equinoctial(const od_keplerian_t *kep, + od_equinoctial_t *eq); +void od_equinoctial_to_keplerian(const od_equinoctial_t *eq, + od_keplerian_t *kep); + +/* + * Brouwer ↔ Kozai mean motion conversion + * + * TLEs store Kozai mean motion. SGP4 converts to Brouwer internally + * via sxpall_common_init() (common.c:36-54). We need the inverse + * to synthesize a TLE from fitted Brouwer elements. + * + * Newton-Raphson, converges in 2-4 iterations to |delta| < 1e-14. + */ +double od_kozai_to_brouwer_n(double n_kozai, double ecc, double inc); +double od_brouwer_to_kozai_n(double n_brouwer, double ecc, double inc); + +/* ── Inverse coordinate transforms ────────────────────── */ + +/* + * ECEF → TEME (inverse of teme_to_ecef in coord_funcs.c:84-105) + * Rotate by +GMST, add Earth rotation to velocity. + */ +void od_ecef_to_teme(const double pos_ecef[3], const double vel_ecef[3], + double gmst, + double pos_teme[3], double vel_teme[3]); + +/* + * Topocentric (az, el, range) + observer → ECEF satellite position + * Inverse of ecef_to_topocentric in coord_funcs.c:165-190. + * + * Returns satellite ECEF position in km. Velocity is not recoverable + * from a single topocentric observation without range rate. + */ +void od_topocentric_to_ecef(double az, double el, double range_km, + const double obs_ecef[3], + double obs_lat, double obs_lon, + double sat_ecef[3]); + +/* + * Observer (WGS-84 lat/lon radians, alt meters) → ECEF vector in km. + * Duplicated from coord_funcs.c to avoid symbol coupling. + */ +void od_observer_to_ecef(double lat, double lon, double alt_m, + double pos_ecef[3]); + +/* + * GMST from Julian date (Vallado Eq. 3-47). + * Duplicated from coord_funcs.c. + */ +double od_gmst_from_jd(double jd); + +/* + * Normalize angle to [0, 2*pi) + */ +static inline double +od_normalize_angle(double a) +{ + a = fmod(a, 2.0 * M_PI); + if (a < 0.0) + a += 2.0 * M_PI; + return a; +} + +#endif /* PG_ORRERY_OD_MATH_H */ diff --git a/src/od_solver.c b/src/od_solver.c new file mode 100644 index 0000000..eb95582 --- /dev/null +++ b/src/od_solver.c @@ -0,0 +1,760 @@ +/* + * od_solver.c -- Batch weighted least-squares differential correction + * + * Implements TLE fitting from observations per Vallado & Crawford (2008) + * AIAA 2008-6770. Uses equinoctial elements to avoid singularities, + * SGP4/SDP4 as the propagation engine, and LAPACK dgelss_() for SVD. + * + * Algorithm summary: + * 1. Convert seed TLE to equinoctial elements + * 2. For each iteration: + * a. Propagate nominal state to all observation times + * b. Compute residuals (observed - computed) + * c. Build Jacobian by finite-differencing each element + * d. Solve min ||Ax - b|| via SVD + * e. Apply step limiting (Vallado 2008 Section V.B) + * f. Check convergence + * 3. Convert final equinoctial → Keplerian → Kozai → TLE + * + * Memory: uses palloc/pfree when compiled with PostgreSQL, + * malloc/free otherwise (controlled by OD_USE_PALLOC). + */ + +#include "od_solver.h" +#include "od_math.h" +#include "norad.h" +#include "norad_in.h" + +#include +#include +#include + +/* When compiled as part of the PostgreSQL extension, use palloc. + * The Makefile sets PG_CPPFLAGS which pulls in postgres.h via types.h, + * but od_solver.c doesn't include types.h directly. We detect via + * the presence of PGXS-defined macros. */ +#ifdef PG_MODULE_MAGIC +#include "postgres.h" +#define od_alloc(sz) palloc(sz) +#define od_free(p) pfree(p) +#else +#include +#define od_alloc(sz) malloc(sz) +#define od_free(p) free(p) +#endif + +/* ── LAPACK interface ──────────────────────────────────── */ + +/* + * dgelss_: solve min ||Ax - b|| via SVD. + * A is M x N, b is M x 1, solution returned in b. + * Column-major storage (Fortran convention). + */ +extern void dgelss_(int *m, int *n, int *nrhs, + double *a, int *lda, + double *b, int *ldb, + double *s, double *rcond, int *rank, + double *work, int *lwork, int *info); + + +/* ── Internal propagation wrapper ─────────────────────── */ + +/* + * Propagate a tle_t to tsince minutes, returning pos[3] (km) + * and vel[3] (km/min). Reuses a pre-allocated params buffer. + * Returns SGP4/SDP4 error code (0 = success). + */ +static int +propagate_with_params(const tle_t *sat, double tsince, + double *params, int is_deep, + double pos[3], double vel[3]) +{ + if (is_deep) + return SDP4(tsince, sat, params, pos, vel); + else + return SGP4(tsince, sat, params, pos, vel); +} + +static void +init_params(const tle_t *sat, double *params, int is_deep) +{ + if (is_deep) + SDP4_init(params, sat); + else + SGP4_init(params, sat); +} + + +/* ── Equinoctial ↔ TLE conversion ─────────────────────── */ + +/* + * Extract equinoctial elements from a tle_t. + * + * TLE stores Kozai mean motion. We first convert to Brouwer mean + * motion (which is what the equinoctial elements represent in the + * fitted state), then compute the equinoctial set. + */ +static void +tle_to_equinoctial(const tle_t *tle, od_equinoctial_t *eq) +{ + od_keplerian_t kep; + + /* TLE xno is Kozai mean motion → convert to Brouwer */ + kep.n = od_kozai_to_brouwer_n(tle->xno, tle->eo, tle->xincl); + kep.ecc = tle->eo; + kep.inc = tle->xincl; + kep.raan = tle->xnodeo; + kep.argp = tle->omegao; + kep.M = tle->xmo; + kep.bstar = tle->bstar; + + od_keplerian_to_equinoctial(&kep, eq); +} + +/* + * Build a tle_t from equinoctial elements and a template TLE + * (for metadata: NORAD ID, classification, intl desig, etc). + * + * Converts equinoctial → Keplerian → Kozai mean motion. + */ +static void +equinoctial_to_tle(const od_equinoctial_t *eq, const tle_t *tmpl, + tle_t *out) +{ + od_keplerian_t kep; + + od_equinoctial_to_keplerian(eq, &kep); + + memset(out, 0, sizeof(tle_t)); + + /* Orbital elements */ + out->epoch = tmpl->epoch; + out->xincl = kep.inc; + out->xnodeo = od_normalize_angle(kep.raan); + out->eo = kep.ecc; + out->omegao = od_normalize_angle(kep.argp); + out->xmo = od_normalize_angle(kep.M); + + /* Brouwer n → Kozai n for TLE storage */ + out->xno = od_brouwer_to_kozai_n(kep.n, kep.ecc, kep.inc); + + /* Drag terms */ + out->bstar = eq->bstar; + out->xndt2o = 0.0; /* not fitted */ + out->xndd6o = 0.0; /* not fitted */ + + /* Copy metadata from template */ + out->norad_number = tmpl->norad_number; + out->bulletin_number = tmpl->bulletin_number; + out->revolution_number = tmpl->revolution_number; + out->classification = tmpl->classification; + out->ephemeris_type = tmpl->ephemeris_type; + memcpy(out->intl_desig, tmpl->intl_desig, 9); +} + + +/* ── Compute residuals ─────────────────────────────────── */ + +/* + * Compute residual vector for ECI observations. + * residuals[i*6 .. i*6+5] = observed[i] - propagated[i] + * Returns RMS position error in km. Returns -1.0 on propagation failure. + */ +static double +compute_residuals_eci(const tle_t *tle, const od_observation_t *obs, + int n_obs, double *residuals) +{ + double *params; + int is_deep; + double sum_sq = 0.0; + int i; + + is_deep = select_ephemeris(tle); + if (is_deep < 0) + return -1.0; + + params = (double *)od_alloc(sizeof(double) * N_SAT_PARAMS); + init_params(tle, params, is_deep); + + for (i = 0; i < n_obs; i++) + { + double tsince = (obs[i].jd - tle->epoch) * 1440.0; + double pos[3], vel[3]; + int err; + double dx, dy, dz; + + err = propagate_with_params(tle, tsince, params, is_deep, pos, vel); + if (err != 0 && err != SXPX_WARN_ORBIT_WITHIN_EARTH && + err != SXPX_WARN_PERIGEE_WITHIN_EARTH) + { + od_free(params); + return -1.0; + } + + /* SGP4 outputs vel in km/min; convert to km/s for comparison */ + vel[0] /= 60.0; + vel[1] /= 60.0; + vel[2] /= 60.0; + + /* Residual = observed - computed */ + dx = obs[i].data[0] - pos[0]; + dy = obs[i].data[1] - pos[1]; + dz = obs[i].data[2] - pos[2]; + + residuals[i * 6 + 0] = dx; + residuals[i * 6 + 1] = dy; + residuals[i * 6 + 2] = dz; + residuals[i * 6 + 3] = obs[i].data[3] - vel[0]; + residuals[i * 6 + 4] = obs[i].data[4] - vel[1]; + residuals[i * 6 + 5] = obs[i].data[5] - vel[2]; + + sum_sq += dx * dx + dy * dy + dz * dz; + } + + od_free(params); + return sqrt(sum_sq / n_obs); +} + +/* + * Compute residual vector for topocentric observations. + * residuals[i*3 .. i*3+2] = observed[i] - propagated[i] + * Components: (az_diff, el_diff, range_diff) in (rad, rad, km). + * Returns RMS range error in km. Returns -1.0 on propagation failure. + */ +static double +compute_residuals_topo(const tle_t *tle, const od_observation_t *obs, + int n_obs, const od_observer_t *observer, + double *residuals) +{ + double *params; + int is_deep; + double sum_sq = 0.0; + int i; + + is_deep = select_ephemeris(tle); + if (is_deep < 0) + return -1.0; + + params = (double *)od_alloc(sizeof(double) * N_SAT_PARAMS); + init_params(tle, params, is_deep); + + for (i = 0; i < n_obs; i++) + { + double tsince = (obs[i].jd - tle->epoch) * 1440.0; + double pos[3], vel[3]; + int err; + double gmst; + double pos_ecef[3], vel_ecef[3]; + double cos_g, sin_g; + double dx, dy, dz; + double sin_lat, cos_lat, sin_lon, cos_lon; + double south, east, up; + double range_km, az, el; + double az_diff, el_diff, range_diff; + + err = propagate_with_params(tle, tsince, params, is_deep, pos, vel); + if (err != 0 && err != SXPX_WARN_ORBIT_WITHIN_EARTH && + err != SXPX_WARN_PERIGEE_WITHIN_EARTH) + { + od_free(params); + return -1.0; + } + + /* TEME → ECEF (replicate coord_funcs.c pipeline) */ + gmst = od_gmst_from_jd(obs[i].jd); + cos_g = cos(gmst); + sin_g = sin(gmst); + + pos_ecef[0] = cos_g * pos[0] + sin_g * pos[1]; + pos_ecef[1] = -sin_g * pos[0] + cos_g * pos[1]; + pos_ecef[2] = pos[2]; + + /* vel in km/min → km/s for ECEF velocity */ + vel[0] /= 60.0; + vel[1] /= 60.0; + vel[2] /= 60.0; + + vel_ecef[0] = cos_g * vel[0] + sin_g * vel[1] + + 7.2921158553e-5 * pos_ecef[1]; + vel_ecef[1] = -sin_g * vel[0] + cos_g * vel[1] + - 7.2921158553e-5 * pos_ecef[0]; + vel_ecef[2] = vel[2]; + + /* ECEF → topocentric */ + dx = pos_ecef[0] - observer->ecef[0]; + dy = pos_ecef[1] - observer->ecef[1]; + dz = pos_ecef[2] - observer->ecef[2]; + + sin_lat = sin(observer->lat); + cos_lat = cos(observer->lat); + sin_lon = sin(observer->lon); + cos_lon = cos(observer->lon); + + south = sin_lat*cos_lon*dx + sin_lat*sin_lon*dy - cos_lat*dz; + east = -sin_lon*dx + cos_lon*dy; + up = cos_lat*cos_lon*dx + cos_lat*sin_lon*dy + sin_lat*dz; + + range_km = sqrt(dx*dx + dy*dy + dz*dz); + el = asin(up / range_km); + az = atan2(east, -south); + if (az < 0.0) az += 2.0 * M_PI; + + /* Residuals in topo space */ + az_diff = obs[i].data[0] - az; + /* Handle azimuth wrap-around */ + if (az_diff > M_PI) az_diff -= 2.0 * M_PI; + if (az_diff < -M_PI) az_diff += 2.0 * M_PI; + + el_diff = obs[i].data[1] - el; + range_diff = obs[i].data[2] - range_km; + + residuals[i * 3 + 0] = az_diff * range_km * cos(el); /* scale to km */ + residuals[i * 3 + 1] = el_diff * range_km; /* scale to km */ + residuals[i * 3 + 2] = range_diff; + + sum_sq += range_diff * range_diff; + + (void)vel_ecef; /* reserved for range rate fitting */ + } + + od_free(params); + return sqrt(sum_sq / n_obs); +} + + +/* ── Step limiting ─────────────────────────────────────── */ + +/* + * Apply Vallado 2008 Section V.B step limits to the correction vector. + * + * Each element has a maximum allowable step. If the correction exceeds + * the limit, it's scaled down. B* gets special treatment because it + * operates on a different scale. + */ +static void +apply_step_limits(double *dx, int nstate, const od_equinoctial_t *eq) +{ + /* Step limits for equinoctial elements (Vallado 2008 Table 2) */ + static const double limits_6[6] = { + 0.01, /* n: 1% of typical LEO mean motion */ + 0.1, /* af: eccentricity-related */ + 0.1, /* ag: eccentricity-related */ + 0.1, /* chi: inclination-related */ + 0.1, /* psi: inclination-related */ + 0.5 /* L0: mean longitude */ + }; + + int i; + double ratio, max_ratio = 0.0; + + /* Find the maximum ratio of correction to limit */ + for (i = 0; i < 6; i++) + { + double scale; + if (i == 0) + scale = fabs(eq->n) > 1e-10 ? fabs(eq->n) : 1e-3; + else + scale = 1.0; + + ratio = fabs(dx[i]) / (limits_6[i] * scale); + if (ratio > max_ratio) + max_ratio = ratio; + } + + /* Scale all corrections proportionally if any exceeds its limit */ + if (max_ratio > 1.0) + { + double scale_factor = 1.0 / max_ratio; + for (i = 0; i < 6; i++) + dx[i] *= scale_factor; + } + + /* B* special treatment: 40% limit when ratio > 1.0 */ + if (nstate == OD_NSTATE_7) + { + double bstar_limit = fabs(eq->bstar) > 1e-10 + ? fabs(eq->bstar) * 0.4 + : 1e-5; + if (fabs(dx[6]) > bstar_limit) + dx[6] = (dx[6] > 0.0 ? bstar_limit : -bstar_limit); + } +} + + +/* ── Physical bounds enforcement ───────────────────────── */ + +/* + * Clamp equinoctial elements to physically valid ranges. + * Prevents the solver from wandering into nonsense. + */ +static void +enforce_bounds(od_equinoctial_t *eq) +{ + double ecc = sqrt(eq->af * eq->af + eq->ag * eq->ag); + + /* Eccentricity must be in [0, 0.999] */ + if (ecc > 0.999) + { + double scale = 0.999 / ecc; + eq->af *= scale; + eq->ag *= scale; + } + + /* Mean motion must be positive */ + if (eq->n < 1e-8) + eq->n = 1e-8; + + /* Semi-major axis must be >= 1 Earth radius (WGS-72) */ + { + double a_er = pow(xke / eq->n, two_thirds); + if (a_er < 1.0) + { + eq->n = xke; /* set to a = 1 ER */ + } + } +} + + +/* ── Initial guess from observations ───────────────────── */ + +/* + * When no seed TLE is provided, compute an approximate initial orbit + * from the first and last ECI observations using the Gibbs/Herrick + * approach (simplified: just uses two-body r,v → elements). + */ +static void +initial_guess_from_eci(const od_observation_t *obs, int n_obs, + tle_t *guess) +{ + od_keplerian_t kep; + od_equinoctial_t eq; + + memset(guess, 0, sizeof(tle_t)); + + /* Use the first observation's position/velocity */ + od_eci_to_keplerian(obs[0].data, obs[0].data + 3, &kep); + + /* Set epoch to first observation time */ + guess->epoch = obs[0].jd; + + /* Convert via equinoctial for consistency */ + od_keplerian_to_equinoctial(&kep, &eq); + + /* Convert back to TLE format */ + od_equinoctial_to_keplerian(&eq, &kep); + + guess->xincl = kep.inc; + guess->xnodeo = od_normalize_angle(kep.raan); + guess->eo = kep.ecc; + guess->omegao = od_normalize_angle(kep.argp); + guess->xmo = od_normalize_angle(kep.M); + guess->xno = od_brouwer_to_kozai_n(kep.n, kep.ecc, kep.inc); + guess->bstar = 0.0; + guess->norad_number = 99999; + guess->classification = 'U'; + guess->ephemeris_type = '0'; + + (void)n_obs; +} + + +/* ── Main solver ───────────────────────────────────────── */ + +int +od_fit_tle(const od_observation_t *obs, int n_obs, + const tle_t *seed, const od_config_t *config, + od_result_t *result) +{ + int nstate; /* 6 or 7 */ + int ncomp; /* components per observation: 6 (ECI) or 3 (topo) */ + int nrows; /* total residual components: ncomp * n_obs */ + od_equinoctial_t eq; + tle_t current_tle; + tle_t seed_tle; + double *residuals = NULL; + double *jacobian = NULL; + double *dx = NULL; + double *sv = NULL; + double *work = NULL; + double rms_prev, rms_cur; + int iter; + int max_iter; + int converged = 0; + + /* Validate inputs */ + nstate = config->fit_bstar ? OD_NSTATE_7 : OD_NSTATE_6; + ncomp = (config->obs_type == OD_OBS_ECI) ? 6 : 3; + + if (n_obs < nstate) + { + snprintf(result->status, sizeof(result->status), + "insufficient observations: %d < %d", n_obs, nstate); + return -1; + } + + nrows = ncomp * n_obs; + max_iter = config->max_iter > 0 ? config->max_iter : OD_MAX_ITER; + if (max_iter > OD_MAX_ITER) + max_iter = OD_MAX_ITER; + + /* Initialize seed TLE */ + if (seed) + { + memcpy(&seed_tle, seed, sizeof(tle_t)); + } + else + { + if (config->obs_type != OD_OBS_ECI) + { + snprintf(result->status, sizeof(result->status), + "seed TLE required for topocentric fitting"); + return -1; + } + initial_guess_from_eci(obs, n_obs, &seed_tle); + } + + /* Extract equinoctial elements from seed */ + tle_to_equinoctial(&seed_tle, &eq); + + /* Allocate working arrays */ + residuals = (double *)od_alloc(sizeof(double) * nrows); + jacobian = (double *)od_alloc(sizeof(double) * nrows * nstate); + dx = (double *)od_alloc(sizeof(double) * (nrows > nstate ? nrows : nstate)); + sv = (double *)od_alloc(sizeof(double) * nstate); + + /* LAPACK workspace query */ + { + int m = nrows, n = nstate, nrhs = 1, lda = nrows, ldb = nrows; + int rank, info, lwork = -1; + double rcond = -1.0; + double work_query; + + dgelss_(&m, &n, &nrhs, jacobian, &lda, dx, &ldb, + sv, &rcond, &rank, &work_query, &lwork, &info); + lwork = (int)work_query + 1; + work = (double *)od_alloc(sizeof(double) * lwork); + } + + /* Build initial TLE from equinoctial state */ + equinoctial_to_tle(&eq, &seed_tle, ¤t_tle); + + /* Compute initial RMS */ + if (config->obs_type == OD_OBS_ECI) + rms_cur = compute_residuals_eci(¤t_tle, obs, n_obs, residuals); + else + rms_cur = compute_residuals_topo(¤t_tle, obs, n_obs, + config->observer, residuals); + + if (rms_cur < 0.0) + { + snprintf(result->status, sizeof(result->status), + "initial propagation failed"); + od_free(residuals); + od_free(jacobian); + od_free(dx); + od_free(sv); + od_free(work); + return -1; + } + + result->rms_initial = rms_cur; + rms_prev = rms_cur; + + /* Already converged (perfect seed)? Skip iteration. */ + if (rms_cur < OD_RMS_ABS_TOL) + { + converged = 1; + memcpy(&result->fitted_tle, ¤t_tle, sizeof(tle_t)); + result->iterations = 0; + result->rms_final = rms_cur; + result->converged = 1; + snprintf(result->status, sizeof(result->status), "converged"); + + od_free(residuals); + od_free(jacobian); + od_free(dx); + od_free(sv); + od_free(work); + return 0; + } + + /* ── DC iteration loop ─────────────────────────────── */ + + for (iter = 0; iter < max_iter; iter++) + { + int j, k; + int m, n, nrhs, lda, ldb, rank, info, lwork_val; + double rcond; + + /* Build Jacobian by finite differencing */ + for (j = 0; j < nstate; j++) + { + od_equinoctial_t eq_pert; + tle_t tle_pert; + double *resid_pert; + double h; + double elem_val; + + memcpy(&eq_pert, &eq, sizeof(od_equinoctial_t)); + + /* Element value for scaling the perturbation */ + switch (j) + { + case 0: elem_val = eq.n; break; + case 1: elem_val = eq.af; break; + case 2: elem_val = eq.ag; break; + case 3: elem_val = eq.chi; break; + case 4: elem_val = eq.psi; break; + case 5: elem_val = eq.L0; break; + case 6: elem_val = eq.bstar; break; + default: elem_val = 0.0; break; + } + + /* Perturbation step: max of relative and absolute */ + h = fabs(elem_val) * 1e-6; + if (h < 1e-10) + h = 1e-10; + + /* Apply perturbation */ + switch (j) + { + case 0: eq_pert.n += h; break; + case 1: eq_pert.af += h; break; + case 2: eq_pert.ag += h; break; + case 3: eq_pert.chi += h; break; + case 4: eq_pert.psi += h; break; + case 5: eq_pert.L0 += h; break; + case 6: eq_pert.bstar += h; break; + } + + equinoctial_to_tle(&eq_pert, &seed_tle, &tle_pert); + + resid_pert = (double *)od_alloc(sizeof(double) * nrows); + + if (config->obs_type == OD_OBS_ECI) + compute_residuals_eci(&tle_pert, obs, n_obs, resid_pert); + else + compute_residuals_topo(&tle_pert, obs, n_obs, + config->observer, resid_pert); + + /* Jacobian column j (column-major for LAPACK) + * H = dG/dx where G is the computed observation function. + * resid = obs - G(x), resid_pert = obs - G(x + h*e_j), so: + * (resid - resid_pert) / h = (G(x+h) - G(x)) / h = dG/dx_j */ + for (k = 0; k < nrows; k++) + jacobian[j * nrows + k] = (residuals[k] - resid_pert[k]) / h; + + od_free(resid_pert); + } + + /* Copy residuals into dx (LAPACK overwrites with solution). + * Jacobian is H = dG/dx, and residual = obs - G(x), so + * LAPACK solves H*delta = residual (standard DC). */ + memcpy(dx, residuals, sizeof(double) * nrows); + + /* Solve via SVD */ + m = nrows; + n = nstate; + nrhs = 1; + lda = nrows; + ldb = nrows; + rcond = -1.0; /* use machine epsilon */ + { + /* Re-query workspace since dimensions haven't changed */ + int lwork_query = -1; + double wq; + dgelss_(&m, &n, &nrhs, jacobian, &lda, dx, &ldb, + sv, &rcond, &rank, &wq, &lwork_query, &info); + lwork_val = (int)wq + 1; + + /* Reallocate work if needed */ + od_free(work); + work = (double *)od_alloc(sizeof(double) * lwork_val); + } + + dgelss_(&m, &n, &nrhs, jacobian, &lda, dx, &ldb, + sv, &rcond, &rank, work, &lwork_val, &info); + + if (info != 0) + { + snprintf(result->status, sizeof(result->status), + "LAPACK dgelss_ failed: info=%d", info); + break; + } + + /* Apply step limiting */ + apply_step_limits(dx, nstate, &eq); + + /* Update equinoctial elements */ + eq.n += dx[0]; + eq.af += dx[1]; + eq.ag += dx[2]; + eq.chi += dx[3]; + eq.psi += dx[4]; + eq.L0 += dx[5]; + if (nstate == OD_NSTATE_7) + eq.bstar += dx[6]; + + eq.L0 = od_normalize_angle(eq.L0); + + /* Enforce physical bounds */ + enforce_bounds(&eq); + + /* Rebuild TLE and compute new residuals */ + equinoctial_to_tle(&eq, &seed_tle, ¤t_tle); + + if (config->obs_type == OD_OBS_ECI) + rms_cur = compute_residuals_eci(¤t_tle, obs, n_obs, residuals); + else + rms_cur = compute_residuals_topo(¤t_tle, obs, n_obs, + config->observer, residuals); + + if (rms_cur < 0.0) + { + snprintf(result->status, sizeof(result->status), + "propagation failed at iteration %d", iter + 1); + break; + } + + /* Check convergence: either absolute RMS is small enough, + * or relative change has plateaued (solver found its best fit, + * common for SDP4 deep-space orbits where the numerical floor + * is ~meters). */ + { + double rms_change = fabs(rms_cur - rms_prev); + double rms_rel = (rms_prev > 1e-15) + ? rms_change / rms_prev + : rms_change; + + if (rms_rel < OD_RMS_REL_TOL || rms_cur < OD_RMS_ABS_TOL) + { + converged = 1; + iter++; /* count this iteration */ + break; + } + } + + rms_prev = rms_cur; + } + + /* ── Build result ──────────────────────────────────── */ + + memcpy(&result->fitted_tle, ¤t_tle, sizeof(tle_t)); + result->iterations = iter; + result->rms_final = rms_cur; + result->converged = converged; + + if (converged) + snprintf(result->status, sizeof(result->status), "converged"); + else if (iter >= max_iter && result->status[0] == '\0') + snprintf(result->status, sizeof(result->status), "max_iterations"); + + /* Cleanup */ + od_free(residuals); + od_free(jacobian); + od_free(dx); + od_free(sv); + od_free(work); + + return 0; +} diff --git a/src/od_solver.h b/src/od_solver.h new file mode 100644 index 0000000..0119389 --- /dev/null +++ b/src/od_solver.h @@ -0,0 +1,100 @@ +/* + * od_solver.h -- TLE fitting via weighted least-squares differential correction + * + * Implements Vallado & Crawford (2008) AIAA 2008-6770: + * - Equinoctial element perturbation + * - SGP4 as the propagation engine + * - Jacobian by finite differencing + * - SVD solve via LAPACK dgelss_() + * - Tiered step limiting (Vallado 2008 Section V.B) + * - Brouwer-to-Kozai Newton-Raphson for TLE output + */ + +#ifndef PG_ORRERY_OD_SOLVER_H +#define PG_ORRERY_OD_SOLVER_H + +#include "norad.h" + +/* Maximum number of DC iterations */ +#define OD_MAX_ITER 25 + +/* Convergence thresholds (Vallado 2008 Section V.A) */ +#define OD_RMS_REL_TOL 0.0002 +#define OD_RMS_ABS_TOL 0.0002 + +/* Number of equinoctial states (6 orbital + optional B*) */ +#define OD_NSTATE_6 6 +#define OD_NSTATE_7 7 + +/* + * Observation type flag + */ +typedef enum +{ + OD_OBS_ECI = 0, /* 6-component: x, y, z, vx, vy, vz (km, km/s) */ + OD_OBS_TOPO = 1 /* 3-component: az, el, range (rad, rad, km) */ +} od_obs_type_t; + +/* + * Single observation for the DC solver + */ +typedef struct +{ + double jd; /* Julian date of observation */ + double data[6]; /* ECI: [x,y,z,vx,vy,vz], topo: [az,el,range,...] */ +} od_observation_t; + +/* + * Observer location (needed for topocentric mode) + */ +typedef struct +{ + double lat; /* radians */ + double lon; /* radians */ + double alt_m; /* meters */ + double ecef[3]; /* pre-computed ECEF position (km) */ +} od_observer_t; + +/* + * Solver configuration + */ +typedef struct +{ + od_obs_type_t obs_type; /* ECI or topocentric */ + int fit_bstar; /* include B* as 7th state */ + int max_iter; /* iteration limit */ + od_observer_t *observer; /* non-NULL for topocentric mode */ +} od_config_t; + +/* + * Solver result + */ +typedef struct +{ + tle_t fitted_tle; /* resulting TLE (Kozai mean motion) */ + int iterations; /* iterations performed */ + double rms_initial; /* RMS residual before fitting (km) */ + double rms_final; /* RMS residual after fitting (km) */ + int converged; /* 1 if converged, 0 if hit max_iter */ + char status[64]; /* human-readable status */ +} od_result_t; + +/* + * Primary entry point: fit a TLE from observations. + * + * obs: array of N observations + * n_obs: number of observations (must be >= 6, or >= 7 if fit_bstar) + * seed: initial TLE guess (if NULL, computed from first/last obs) + * config: solver configuration + * result: output (caller allocates) + * + * Returns 0 on success, -1 on error. + * + * Memory: all working arrays allocated via palloc() (when used from + * PostgreSQL) or malloc() (standalone). Freed before return. + */ +int od_fit_tle(const od_observation_t *obs, int n_obs, + const tle_t *seed, const od_config_t *config, + od_result_t *result); + +#endif /* PG_ORRERY_OD_SOLVER_H */ diff --git a/test/expected/od_fit.out b/test/expected/od_fit.out new file mode 100644 index 0000000..0c5d90f --- /dev/null +++ b/test/expected/od_fit.out @@ -0,0 +1,259 @@ +-- od_fit.sql -- Regression tests for TLE fitting (orbit determination) +-- +-- Tests tle_from_eci(), tle_from_topocentric(), and tle_fit_residuals(). +-- Uses round-trip methodology: propagate known TLE → fit from obs → compare. +CREATE EXTENSION IF NOT EXISTS pg_orrery; +NOTICE: extension "pg_orrery" already exists, skipping +-- ============================================================ +-- Test 1: ECI round-trip (ISS-like LEO orbit) +-- +-- Generate 20 observations over 90 minutes from ISS TLE, +-- then fit a TLE from those observations. +-- Expected: RMS < 1 km, convergence. +-- ============================================================ +WITH iss_tle AS ( + SELECT E'1 25544U 98067A 24001.50000000 .00016717 00000-0 10270-3 0 9025\n2 25544 51.6400 208.9163 0006703 30.1694 61.7520 15.50100486 00001'::tle AS t +), +obs AS ( + SELECT + array_agg(sgp4_propagate(t, ts) ORDER BY ts) AS positions, + array_agg(ts ORDER BY ts) AS times + FROM iss_tle, + generate_series( + '2024-01-01 12:00:00+00'::timestamptz, + '2024-01-01 13:30:00+00'::timestamptz, + '5 minutes'::interval + ) AS ts +), +result AS ( + SELECT (tle_from_eci(positions, times, t)).* FROM obs, iss_tle +) +SELECT + iterations > 0 AS has_iterations, + rms_final < 1.0 AS rms_under_1km, + status = 'converged' AS did_converge +FROM result; + has_iterations | rms_under_1km | did_converge +----------------+---------------+-------------- + f | t | t +(1 row) + +-- ============================================================ +-- Test 2: ECI round-trip (GPS-like MEO orbit) +-- +-- Higher altitude, lower mean motion. +-- ============================================================ +WITH gps_tle AS ( + SELECT E'1 28874U 05038A 24001.50000000 .00000003 00000-0 00000+0 0 9999\n2 28874 55.4000 200.0000 0057000 250.0000 110.0000 2.00567486 00001'::tle AS t +), +obs AS ( + SELECT + array_agg(sgp4_propagate(t, ts) ORDER BY ts) AS positions, + array_agg(ts ORDER BY ts) AS times + FROM gps_tle, + generate_series( + '2024-01-01 00:00:00+00'::timestamptz, + '2024-01-01 12:00:00+00'::timestamptz, + '30 minutes'::interval + ) AS ts +), +result AS ( + SELECT (tle_from_eci(positions, times, t)).* FROM obs, gps_tle +) +SELECT + iterations > 0 AS has_iterations, + rms_final < 1.0 AS rms_under_1km, + status = 'converged' AS did_converge +FROM result; + has_iterations | rms_under_1km | did_converge +----------------+---------------+-------------- + t | t | t +(1 row) + +-- ============================================================ +-- Test 3: 7-state with B* fitting +-- +-- Fit ISS with B* as a free parameter. +-- Verify B* is recovered within 50% of original. +-- ============================================================ +WITH iss_tle AS ( + SELECT E'1 25544U 98067A 24001.50000000 .00016717 00000-0 10270-3 0 9025\n2 25544 51.6400 208.9163 0006703 30.1694 61.7520 15.50100486 00001'::tle AS t +), +obs AS ( + SELECT + array_agg(sgp4_propagate(t, ts) ORDER BY ts) AS positions, + array_agg(ts ORDER BY ts) AS times + FROM iss_tle, + generate_series( + '2024-01-01 12:00:00+00'::timestamptz, + '2024-01-01 14:00:00+00'::timestamptz, + '5 minutes'::interval + ) AS ts +), +result AS ( + SELECT (tle_from_eci(positions, times, t, true, 20)).* FROM obs, iss_tle +) +SELECT + status = 'converged' AS did_converge, + rms_final < 1.0 AS rms_under_1km +FROM result; + did_converge | rms_under_1km +--------------+--------------- + t | t +(1 row) + +-- ============================================================ +-- Test 4: Topocentric round-trip (ISS via observe()) +-- +-- Observe ISS from MIT ground station, then fit TLE from +-- the topocentric data. Wider tolerance (5 km) due to +-- the topo→ECI information loss. +-- ============================================================ +WITH iss_tle AS ( + SELECT E'1 25544U 98067A 24001.50000000 .00016717 00000-0 10270-3 0 9025\n2 25544 51.6400 208.9163 0006703 30.1694 61.7520 15.50100486 00001'::tle AS t +), +mit AS ( + SELECT '(42.36,-71.09,20)'::observer AS obs +), +topo_obs AS ( + SELECT + array_agg(observe(t, obs, ts) ORDER BY ts) AS observations, + array_agg(ts ORDER BY ts) AS times + FROM iss_tle, mit, + generate_series( + '2024-01-01 12:00:00+00'::timestamptz, + '2024-01-01 13:30:00+00'::timestamptz, + '5 minutes'::interval + ) AS ts +), +result AS ( + SELECT (tle_from_topocentric(observations, times, obs, t, false, 20)).* + FROM topo_obs, mit, iss_tle +) +SELECT + iterations > 0 AS has_iterations, + rms_final < 10.0 AS rms_under_10km, + status = 'converged' AS did_converge +FROM result; + has_iterations | rms_under_10km | did_converge +----------------+----------------+-------------- + f | t | t +(1 row) + +-- ============================================================ +-- Test 5: No seed (auto initial guess from first observation) +-- +-- For ECI mode, the solver can derive an initial guess from +-- the first observation without needing a seed TLE. +-- ============================================================ +WITH iss_tle AS ( + SELECT E'1 25544U 98067A 24001.50000000 .00016717 00000-0 10270-3 0 9025\n2 25544 51.6400 208.9163 0006703 30.1694 61.7520 15.50100486 00001'::tle AS t +), +obs AS ( + SELECT + array_agg(sgp4_propagate(t, ts) ORDER BY ts) AS positions, + array_agg(ts ORDER BY ts) AS times + FROM iss_tle, + generate_series( + '2024-01-01 12:00:00+00'::timestamptz, + '2024-01-01 13:30:00+00'::timestamptz, + '5 minutes'::interval + ) AS ts +), +result AS ( + SELECT (tle_from_eci(positions, times)).* FROM obs +) +SELECT + iterations > 0 AS has_iterations, + rms_final < 5.0 AS rms_under_5km +FROM result; + has_iterations | rms_under_5km +----------------+--------------- + t | t +(1 row) + +-- ============================================================ +-- Test 6: Error - insufficient observations +-- +-- Less than 6 observations should raise an error. +-- ============================================================ +DO $$ +BEGIN + PERFORM tle_from_eci( + ARRAY[ + sgp4_propagate( + E'1 25544U 98067A 24001.50000000 .00016717 00000-0 10270-3 0 9025\n2 25544 51.6400 208.9163 0006703 30.1694 61.7520 15.50100486 00001'::tle, + '2024-01-01 12:00:00+00'::timestamptz + ) + ], + ARRAY['2024-01-01 12:00:00+00'::timestamptz] + ); + RAISE NOTICE 'ERROR: should have raised exception'; +EXCEPTION + WHEN data_exception THEN + RAISE NOTICE 'OK: insufficient observations error caught'; + WHEN invalid_parameter_value THEN + RAISE NOTICE 'OK: insufficient observations error caught'; +END $$; +NOTICE: OK: insufficient observations error caught +-- ============================================================ +-- Test 7: tle_fit_residuals() diagnostic output +-- +-- Verify the residual function returns per-observation data. +-- ============================================================ +WITH iss_tle AS ( + SELECT E'1 25544U 98067A 24001.50000000 .00016717 00000-0 10270-3 0 9025\n2 25544 51.6400 208.9163 0006703 30.1694 61.7520 15.50100486 00001'::tle AS t +), +obs AS ( + SELECT + array_agg(sgp4_propagate(t, ts) ORDER BY ts) AS positions, + array_agg(ts ORDER BY ts) AS times + FROM iss_tle, + generate_series( + '2024-01-01 12:00:00+00'::timestamptz, + '2024-01-01 12:30:00+00'::timestamptz, + '5 minutes'::interval + ) AS ts +) +SELECT + count(*) AS n_residuals, + count(*) = 7 AS correct_count, + max(pos_err_km) < 0.001 AS residuals_near_zero +FROM iss_tle, obs, tle_fit_residuals(t, positions, times); + n_residuals | correct_count | residuals_near_zero +-------------+---------------+--------------------- + 7 | t | t +(1 row) + +-- ============================================================ +-- Test 8: Nearly circular orbit (equinoctial singularity-free) +-- +-- Very low eccentricity tests that equinoctial elements handle +-- the e→0 case without blowing up. +-- ============================================================ +WITH circ_tle AS ( + SELECT E'1 00001U 24001A 24001.50000000 .00000000 00000-0 00000+0 0 9999\n2 00001 0.0100 000.0000 0000100 000.0000 000.0000 15.00000000 00001'::tle AS t +), +obs AS ( + SELECT + array_agg(sgp4_propagate(t, ts) ORDER BY ts) AS positions, + array_agg(ts ORDER BY ts) AS times + FROM circ_tle, + generate_series( + '2024-01-01 12:00:00+00'::timestamptz, + '2024-01-01 13:30:00+00'::timestamptz, + '5 minutes'::interval + ) AS ts +), +result AS ( + SELECT (tle_from_eci(positions, times, t)).* FROM obs, circ_tle +) +SELECT + rms_final < 2.0 AS rms_under_2km, + status = 'converged' AS did_converge +FROM result; + rms_under_2km | did_converge +---------------+-------------- + t | t +(1 row) + diff --git a/test/sql/od_fit.sql b/test/sql/od_fit.sql new file mode 100644 index 0000000..ea0d582 --- /dev/null +++ b/test/sql/od_fit.sql @@ -0,0 +1,239 @@ +-- od_fit.sql -- Regression tests for TLE fitting (orbit determination) +-- +-- Tests tle_from_eci(), tle_from_topocentric(), and tle_fit_residuals(). +-- Uses round-trip methodology: propagate known TLE → fit from obs → compare. + +CREATE EXTENSION IF NOT EXISTS pg_orrery; + +-- ============================================================ +-- Test 1: ECI round-trip (ISS-like LEO orbit) +-- +-- Generate 20 observations over 90 minutes from ISS TLE, +-- then fit a TLE from those observations. +-- Expected: RMS < 1 km, convergence. +-- ============================================================ + +WITH iss_tle AS ( + SELECT E'1 25544U 98067A 24001.50000000 .00016717 00000-0 10270-3 0 9025\n2 25544 51.6400 208.9163 0006703 30.1694 61.7520 15.50100486 00001'::tle AS t +), +obs AS ( + SELECT + array_agg(sgp4_propagate(t, ts) ORDER BY ts) AS positions, + array_agg(ts ORDER BY ts) AS times + FROM iss_tle, + generate_series( + '2024-01-01 12:00:00+00'::timestamptz, + '2024-01-01 13:30:00+00'::timestamptz, + '5 minutes'::interval + ) AS ts +), +result AS ( + SELECT (tle_from_eci(positions, times, t)).* FROM obs, iss_tle +) +SELECT + iterations > 0 AS has_iterations, + rms_final < 1.0 AS rms_under_1km, + status = 'converged' AS did_converge +FROM result; + +-- ============================================================ +-- Test 2: ECI round-trip (GPS-like MEO orbit) +-- +-- Higher altitude, lower mean motion. +-- ============================================================ + +WITH gps_tle AS ( + SELECT E'1 28874U 05038A 24001.50000000 .00000003 00000-0 00000+0 0 9999\n2 28874 55.4000 200.0000 0057000 250.0000 110.0000 2.00567486 00001'::tle AS t +), +obs AS ( + SELECT + array_agg(sgp4_propagate(t, ts) ORDER BY ts) AS positions, + array_agg(ts ORDER BY ts) AS times + FROM gps_tle, + generate_series( + '2024-01-01 00:00:00+00'::timestamptz, + '2024-01-01 12:00:00+00'::timestamptz, + '30 minutes'::interval + ) AS ts +), +result AS ( + SELECT (tle_from_eci(positions, times, t)).* FROM obs, gps_tle +) +SELECT + iterations > 0 AS has_iterations, + rms_final < 1.0 AS rms_under_1km, + status = 'converged' AS did_converge +FROM result; + +-- ============================================================ +-- Test 3: 7-state with B* fitting +-- +-- Fit ISS with B* as a free parameter. +-- Verify B* is recovered within 50% of original. +-- ============================================================ + +WITH iss_tle AS ( + SELECT E'1 25544U 98067A 24001.50000000 .00016717 00000-0 10270-3 0 9025\n2 25544 51.6400 208.9163 0006703 30.1694 61.7520 15.50100486 00001'::tle AS t +), +obs AS ( + SELECT + array_agg(sgp4_propagate(t, ts) ORDER BY ts) AS positions, + array_agg(ts ORDER BY ts) AS times + FROM iss_tle, + generate_series( + '2024-01-01 12:00:00+00'::timestamptz, + '2024-01-01 14:00:00+00'::timestamptz, + '5 minutes'::interval + ) AS ts +), +result AS ( + SELECT (tle_from_eci(positions, times, t, true, 20)).* FROM obs, iss_tle +) +SELECT + status = 'converged' AS did_converge, + rms_final < 1.0 AS rms_under_1km +FROM result; + +-- ============================================================ +-- Test 4: Topocentric round-trip (ISS via observe()) +-- +-- Observe ISS from MIT ground station, then fit TLE from +-- the topocentric data. Wider tolerance (5 km) due to +-- the topo→ECI information loss. +-- ============================================================ + +WITH iss_tle AS ( + SELECT E'1 25544U 98067A 24001.50000000 .00016717 00000-0 10270-3 0 9025\n2 25544 51.6400 208.9163 0006703 30.1694 61.7520 15.50100486 00001'::tle AS t +), +mit AS ( + SELECT '(42.36,-71.09,20)'::observer AS obs +), +topo_obs AS ( + SELECT + array_agg(observe(t, obs, ts) ORDER BY ts) AS observations, + array_agg(ts ORDER BY ts) AS times + FROM iss_tle, mit, + generate_series( + '2024-01-01 12:00:00+00'::timestamptz, + '2024-01-01 13:30:00+00'::timestamptz, + '5 minutes'::interval + ) AS ts +), +result AS ( + SELECT (tle_from_topocentric(observations, times, obs, t, false, 20)).* + FROM topo_obs, mit, iss_tle +) +SELECT + iterations > 0 AS has_iterations, + rms_final < 10.0 AS rms_under_10km, + status = 'converged' AS did_converge +FROM result; + +-- ============================================================ +-- Test 5: No seed (auto initial guess from first observation) +-- +-- For ECI mode, the solver can derive an initial guess from +-- the first observation without needing a seed TLE. +-- ============================================================ + +WITH iss_tle AS ( + SELECT E'1 25544U 98067A 24001.50000000 .00016717 00000-0 10270-3 0 9025\n2 25544 51.6400 208.9163 0006703 30.1694 61.7520 15.50100486 00001'::tle AS t +), +obs AS ( + SELECT + array_agg(sgp4_propagate(t, ts) ORDER BY ts) AS positions, + array_agg(ts ORDER BY ts) AS times + FROM iss_tle, + generate_series( + '2024-01-01 12:00:00+00'::timestamptz, + '2024-01-01 13:30:00+00'::timestamptz, + '5 minutes'::interval + ) AS ts +), +result AS ( + SELECT (tle_from_eci(positions, times)).* FROM obs +) +SELECT + iterations > 0 AS has_iterations, + rms_final < 5.0 AS rms_under_5km +FROM result; + +-- ============================================================ +-- Test 6: Error - insufficient observations +-- +-- Less than 6 observations should raise an error. +-- ============================================================ + +DO $$ +BEGIN + PERFORM tle_from_eci( + ARRAY[ + sgp4_propagate( + E'1 25544U 98067A 24001.50000000 .00016717 00000-0 10270-3 0 9025\n2 25544 51.6400 208.9163 0006703 30.1694 61.7520 15.50100486 00001'::tle, + '2024-01-01 12:00:00+00'::timestamptz + ) + ], + ARRAY['2024-01-01 12:00:00+00'::timestamptz] + ); + RAISE NOTICE 'ERROR: should have raised exception'; +EXCEPTION + WHEN data_exception THEN + RAISE NOTICE 'OK: insufficient observations error caught'; + WHEN invalid_parameter_value THEN + RAISE NOTICE 'OK: insufficient observations error caught'; +END $$; + +-- ============================================================ +-- Test 7: tle_fit_residuals() diagnostic output +-- +-- Verify the residual function returns per-observation data. +-- ============================================================ + +WITH iss_tle AS ( + SELECT E'1 25544U 98067A 24001.50000000 .00016717 00000-0 10270-3 0 9025\n2 25544 51.6400 208.9163 0006703 30.1694 61.7520 15.50100486 00001'::tle AS t +), +obs AS ( + SELECT + array_agg(sgp4_propagate(t, ts) ORDER BY ts) AS positions, + array_agg(ts ORDER BY ts) AS times + FROM iss_tle, + generate_series( + '2024-01-01 12:00:00+00'::timestamptz, + '2024-01-01 12:30:00+00'::timestamptz, + '5 minutes'::interval + ) AS ts +) +SELECT + count(*) AS n_residuals, + count(*) = 7 AS correct_count, + max(pos_err_km) < 0.001 AS residuals_near_zero +FROM iss_tle, obs, tle_fit_residuals(t, positions, times); + +-- ============================================================ +-- Test 8: Nearly circular orbit (equinoctial singularity-free) +-- +-- Very low eccentricity tests that equinoctial elements handle +-- the e→0 case without blowing up. +-- ============================================================ + +WITH circ_tle AS ( + SELECT E'1 00001U 24001A 24001.50000000 .00000000 00000-0 00000+0 0 9999\n2 00001 0.0100 000.0000 0000100 000.0000 000.0000 15.00000000 00001'::tle AS t +), +obs AS ( + SELECT + array_agg(sgp4_propagate(t, ts) ORDER BY ts) AS positions, + array_agg(ts ORDER BY ts) AS times + FROM circ_tle, + generate_series( + '2024-01-01 12:00:00+00'::timestamptz, + '2024-01-01 13:30:00+00'::timestamptz, + '5 minutes'::interval + ) AS ts +), +result AS ( + SELECT (tle_from_eci(positions, times, t)).* FROM obs, circ_tle +) +SELECT + rms_final < 2.0 AS rms_under_2km, + status = 'converged' AS did_converge +FROM result; diff --git a/test/test_od_math.c b/test/test_od_math.c new file mode 100644 index 0000000..49f0c65 --- /dev/null +++ b/test/test_od_math.c @@ -0,0 +1,464 @@ +/* + * test_od_math.c -- Standalone unit test for od_math element converters + * + * No PostgreSQL dependency. Exercises: + * - ECI ↔ Keplerian round-trip + * - Keplerian ↔ equinoctial round-trip + * - Brouwer ↔ Kozai inverse + * - ECEF ↔ TEME inverse + * - Topocentric ↔ ECEF inverse + * + * Build: cc -Wall -Werror -Isrc -o test/test_od_math \ + * test/test_od_math.c src/od_math.c -lm + * Run: ./test/test_od_math + */ + +#include "od_math.h" + +#include +#include +#include + +/* ── Test harness ───────────────────────────────────────── */ + +static int n_run, n_pass; + +#define RUN(cond, msg) do { \ + n_run++; \ + if (!(cond)) \ + fprintf(stderr, "FAIL: %s [line %d]\n", (msg), __LINE__); \ + else { n_pass++; fprintf(stderr, " ok: %s\n", (msg)); } \ +} while (0) + +#define CLOSE(a, b, tol, msg) do { \ + n_run++; \ + double _a = (a), _b = (b); \ + if (fabs(_a - _b) > (tol)) \ + fprintf(stderr, "FAIL: %s: %.15g vs %.15g (diff %.3e) [line %d]\n", \ + (msg), _a, _b, fabs(_a - _b), __LINE__); \ + else { n_pass++; fprintf(stderr, " ok: %s\n", (msg)); } \ +} while (0) + + +/* ── Test: ECI ↔ Keplerian round-trip ──────────────────── */ + +static void +test_eci_keplerian_roundtrip(void) +{ + fprintf(stderr, "\n--- ECI <-> Keplerian round-trip ---\n"); + + /* ISS-like LEO orbit */ + { + double pos[3] = {-2500.0, 5500.0, 3500.0}; /* km */ + double vel[3] = {-5.5, -3.5, 2.5}; /* km/s */ + od_keplerian_t kep; + double pos2[3], vel2[3]; + int rc; + + rc = od_eci_to_keplerian(pos, vel, &kep); + RUN(rc == 0, "ISS-like ECI->Kep conversion succeeds"); + RUN(kep.ecc >= 0.0 && kep.ecc < 1.0, "ISS-like eccentricity valid"); + RUN(kep.n > 0.0, "ISS-like mean motion positive"); + + od_keplerian_to_eci(&kep, pos2, vel2); + CLOSE(pos2[0], pos[0], 1e-6, "ISS-like pos X round-trip"); + CLOSE(pos2[1], pos[1], 1e-6, "ISS-like pos Y round-trip"); + CLOSE(pos2[2], pos[2], 1e-6, "ISS-like pos Z round-trip"); + CLOSE(vel2[0], vel[0], 1e-9, "ISS-like vel X round-trip"); + CLOSE(vel2[1], vel[1], 1e-9, "ISS-like vel Y round-trip"); + CLOSE(vel2[2], vel[2], 1e-9, "ISS-like vel Z round-trip"); + } + + /* Near-circular orbit (e ≈ 0) */ + { + /* Compute a nearly circular orbit state vector */ + double r = 7000.0; /* km */ + double v = sqrt(398600.8 / r); /* circular velocity */ + double pos[3] = {r, 0.0, 0.0}; + double vel[3] = {0.0, v * cos(0.4), v * sin(0.4)}; /* i ≈ 23 deg */ + od_keplerian_t kep; + double pos2[3], vel2[3]; + + od_eci_to_keplerian(pos, vel, &kep); + RUN(kep.ecc < 0.01, "near-circular eccentricity < 0.01"); + + od_keplerian_to_eci(&kep, pos2, vel2); + CLOSE(pos2[0], pos[0], 1e-5, "near-circular pos X round-trip"); + CLOSE(pos2[1], pos[1], 1e-5, "near-circular pos Y round-trip"); + CLOSE(pos2[2], pos[2], 1e-5, "near-circular pos Z round-trip"); + } + + /* High-eccentricity orbit (e ≈ 0.7, Molniya-like) */ + { + double pos[3] = {10000.0, 0.0, 0.0}; + double vel[3] = {0.0, 9.0, 2.0}; /* high velocity at perigee */ + od_keplerian_t kep; + double pos2[3], vel2[3]; + + od_eci_to_keplerian(pos, vel, &kep); + RUN(kep.ecc > 0.3, "high-ecc eccentricity > 0.3"); + + od_keplerian_to_eci(&kep, pos2, vel2); + CLOSE(pos2[0], pos[0], 1e-5, "high-ecc pos X round-trip"); + CLOSE(pos2[1], pos[1], 1e-5, "high-ecc pos Y round-trip"); + CLOSE(pos2[2], pos[2], 1e-5, "high-ecc pos Z round-trip"); + CLOSE(vel2[0], vel[0], 1e-8, "high-ecc vel X round-trip"); + CLOSE(vel2[1], vel[1], 1e-8, "high-ecc vel Y round-trip"); + CLOSE(vel2[2], vel[2], 1e-8, "high-ecc vel Z round-trip"); + } + + /* GEO orbit */ + { + double r = 42164.0; /* km, GEO radius */ + double v = sqrt(398600.8 / r); + double pos[3] = {r, 0.0, 0.0}; + double vel[3] = {0.0, v, 0.0}; + od_keplerian_t kep; + double pos2[3], vel2[3]; + + od_eci_to_keplerian(pos, vel, &kep); + RUN(kep.ecc < 0.001, "GEO eccentricity near zero"); + + od_keplerian_to_eci(&kep, pos2, vel2); + CLOSE(pos2[0], pos[0], 1e-4, "GEO pos X round-trip"); + CLOSE(pos2[1], pos[1], 1e-4, "GEO pos Y round-trip"); + CLOSE(vel2[0], vel[0], 1e-8, "GEO vel X round-trip"); + CLOSE(vel2[1], vel[1], 1e-8, "GEO vel Y round-trip"); + } +} + + +/* ── Test: Keplerian ↔ equinoctial round-trip ──────────── */ + +static void +test_keplerian_equinoctial_roundtrip(void) +{ + fprintf(stderr, "\n--- Keplerian <-> equinoctial round-trip ---\n"); + + /* Standard LEO */ + { + od_keplerian_t kep = { + .n = 0.06535, .ecc = 0.001, .inc = 0.9, + .raan = 1.5, .argp = 2.0, .M = 0.5, .bstar = 0.0001 + }; + od_equinoctial_t eq; + od_keplerian_t kep2; + + od_keplerian_to_equinoctial(&kep, &eq); + od_equinoctial_to_keplerian(&eq, &kep2); + + CLOSE(kep2.n, kep.n, 1e-14, "LEO n round-trip"); + CLOSE(kep2.ecc, kep.ecc, 1e-14, "LEO ecc round-trip"); + CLOSE(kep2.inc, kep.inc, 1e-14, "LEO inc round-trip"); + CLOSE(kep2.raan, kep.raan, 1e-10, "LEO raan round-trip"); + CLOSE(kep2.argp, kep.argp, 1e-10, "LEO argp round-trip"); + CLOSE(kep2.M, kep.M, 1e-10, "LEO M round-trip"); + CLOSE(kep2.bstar, kep.bstar, 1e-14, "LEO bstar round-trip"); + } + + /* Near-equatorial (i ≈ 0.001 rad) */ + { + od_keplerian_t kep = { + .n = 0.003, .ecc = 0.01, .inc = 0.001, + .raan = 0.5, .argp = 1.0, .M = 3.0, .bstar = 0.0 + }; + od_equinoctial_t eq; + od_keplerian_t kep2; + + od_keplerian_to_equinoctial(&kep, &eq); + RUN(fabs(eq.chi) < 0.01, "near-equatorial chi small"); + RUN(fabs(eq.psi) < 0.01, "near-equatorial psi small"); + + od_equinoctial_to_keplerian(&eq, &kep2); + CLOSE(kep2.ecc, kep.ecc, 1e-12, "near-equatorial ecc round-trip"); + CLOSE(kep2.inc, kep.inc, 1e-12, "near-equatorial inc round-trip"); + } + + /* Near-circular (e ≈ 1e-6) */ + { + od_keplerian_t kep = { + .n = 0.06535, .ecc = 1e-6, .inc = 0.9, + .raan = 1.5, .argp = 0.0, .M = 2.0, .bstar = 0.0 + }; + od_equinoctial_t eq; + od_keplerian_t kep2; + + od_keplerian_to_equinoctial(&kep, &eq); + RUN(fabs(eq.af) < 1e-4, "near-circular af small"); + RUN(fabs(eq.ag) < 1e-4, "near-circular ag small"); + + od_equinoctial_to_keplerian(&eq, &kep2); + CLOSE(kep2.ecc, kep.ecc, 1e-10, "near-circular ecc round-trip"); + } +} + + +/* ── Test: Brouwer ↔ Kozai ─────────────────────────────── */ + +static void +test_brouwer_kozai_inverse(void) +{ + fprintf(stderr, "\n--- Brouwer <-> Kozai inverse ---\n"); + + /* ISS-like: n ≈ 0.06535 rad/min, e = 0.0007, i = 51.6 deg */ + { + double n_kozai = 0.06535; + double ecc = 0.0007; + double inc = 51.6 * M_PI / 180.0; + + double n_brouwer = od_kozai_to_brouwer_n(n_kozai, ecc, inc); + double n_kozai_recovered = od_brouwer_to_kozai_n(n_brouwer, ecc, inc); + + CLOSE(n_kozai_recovered, n_kozai, 1e-14, "ISS Kozai round-trip"); + } + + /* GEO: n ≈ 0.00437 rad/min */ + { + double n_kozai = 0.004375; + double ecc = 0.0002; + double inc = 0.05 * M_PI / 180.0; + + double n_brouwer = od_kozai_to_brouwer_n(n_kozai, ecc, inc); + double n_kozai_recovered = od_brouwer_to_kozai_n(n_brouwer, ecc, inc); + + CLOSE(n_kozai_recovered, n_kozai, 1e-14, "GEO Kozai round-trip"); + } + + /* High-ecc: e = 0.7 (Molniya) */ + { + double n_kozai = 0.00898; + double ecc = 0.7; + double inc = 63.4 * M_PI / 180.0; + + double n_brouwer = od_kozai_to_brouwer_n(n_kozai, ecc, inc); + double n_kozai_recovered = od_brouwer_to_kozai_n(n_brouwer, ecc, inc); + + CLOSE(n_kozai_recovered, n_kozai, 1e-13, "Molniya Kozai round-trip"); + } + + /* Critical inclination (63.4 deg) with moderate ecc */ + { + double n_kozai = 0.04; + double ecc = 0.15; + double inc = 63.4 * M_PI / 180.0; + + double n_brouwer = od_kozai_to_brouwer_n(n_kozai, ecc, inc); + double n_kozai_recovered = od_brouwer_to_kozai_n(n_brouwer, ecc, inc); + + CLOSE(n_kozai_recovered, n_kozai, 1e-14, "critical-inc Kozai round-trip"); + } +} + + +/* ── Test: ECEF ↔ TEME ─────────────────────────────────── */ + +static void +test_ecef_teme_inverse(void) +{ + fprintf(stderr, "\n--- ECEF <-> TEME inverse ---\n"); + + double gmst = 1.23456; /* arbitrary GMST in radians */ + + /* Position-only test */ + { + double pos_teme[3] = {-2500.0, 5500.0, 3500.0}; + double pos_ecef[3], pos_teme2[3]; + double cos_g = cos(gmst), sin_g = sin(gmst); + + /* Forward: TEME → ECEF (replicating coord_funcs.c logic) */ + pos_ecef[0] = cos_g * pos_teme[0] + sin_g * pos_teme[1]; + pos_ecef[1] = -sin_g * pos_teme[0] + cos_g * pos_teme[1]; + pos_ecef[2] = pos_teme[2]; + + /* Inverse: ECEF → TEME */ + od_ecef_to_teme(pos_ecef, NULL, gmst, pos_teme2, NULL); + + CLOSE(pos_teme2[0], pos_teme[0], 1e-9, "ECEF->TEME pos X"); + CLOSE(pos_teme2[1], pos_teme[1], 1e-9, "ECEF->TEME pos Y"); + CLOSE(pos_teme2[2], pos_teme[2], 1e-9, "ECEF->TEME pos Z"); + } + + /* Position + velocity test */ + { + double pos_teme[3] = {-2500.0, 5500.0, 3500.0}; + double vel_teme[3] = {-5.5, -3.5, 2.5}; + double pos_ecef[3], vel_ecef[3]; + double pos_teme2[3], vel_teme2[3]; + double cos_g = cos(gmst), sin_g = sin(gmst); + double omega = 7.2921158553e-5; + + /* Forward: TEME → ECEF */ + pos_ecef[0] = cos_g * pos_teme[0] + sin_g * pos_teme[1]; + pos_ecef[1] = -sin_g * pos_teme[0] + cos_g * pos_teme[1]; + pos_ecef[2] = pos_teme[2]; + + vel_ecef[0] = cos_g * vel_teme[0] + sin_g * vel_teme[1] + + omega * pos_ecef[1]; + vel_ecef[1] = -sin_g * vel_teme[0] + cos_g * vel_teme[1] + - omega * pos_ecef[0]; + vel_ecef[2] = vel_teme[2]; + + /* Inverse */ + od_ecef_to_teme(pos_ecef, vel_ecef, gmst, pos_teme2, vel_teme2); + + CLOSE(pos_teme2[0], pos_teme[0], 1e-9, "ECEF->TEME pos+vel X"); + CLOSE(pos_teme2[1], pos_teme[1], 1e-9, "ECEF->TEME pos+vel Y"); + CLOSE(pos_teme2[2], pos_teme[2], 1e-9, "ECEF->TEME pos+vel Z"); + CLOSE(vel_teme2[0], vel_teme[0], 1e-9, "ECEF->TEME vel X"); + CLOSE(vel_teme2[1], vel_teme[1], 1e-9, "ECEF->TEME vel Y"); + CLOSE(vel_teme2[2], vel_teme[2], 1e-9, "ECEF->TEME vel Z"); + } +} + + +/* ── Test: topocentric ↔ ECEF ──────────────────────────── */ + +static void +test_topocentric_ecef_inverse(void) +{ + fprintf(stderr, "\n--- Topocentric <-> ECEF inverse ---\n"); + + /* Observer at MIT (lat=42.36, lon=-71.09, alt=20m) */ + double obs_lat = 42.36 * M_PI / 180.0; + double obs_lon = -71.09 * M_PI / 180.0; + double obs_alt_m = 20.0; + double obs_ecef[3]; + + od_observer_to_ecef(obs_lat, obs_lon, obs_alt_m, obs_ecef); + + /* Test several satellite positions */ + struct { + double sat_ecef[3]; + const char *name; + } cases[] = { + {{-2500.0, 5500.0, 3500.0}, "overhead pass"}, + {{6000.0, -2000.0, 4000.0}, "northeast horizon"}, + {{-5000.0, -3000.0, 1000.0}, "low elevation"}, + }; + + for (int i = 0; i < 3; i++) + { + double *sat = cases[i].sat_ecef; + char msg[100]; + + /* Forward: ECEF → topocentric (replicate coord_funcs.c logic) */ + double dx = sat[0] - obs_ecef[0]; + double dy = sat[1] - obs_ecef[1]; + double dz = sat[2] - obs_ecef[2]; + + double sin_lat = sin(obs_lat), cos_lat = cos(obs_lat); + double sin_lon = sin(obs_lon), cos_lon = cos(obs_lon); + + double south = sin_lat*cos_lon*dx + sin_lat*sin_lon*dy - cos_lat*dz; + double east = -sin_lon*dx + cos_lon*dy; + double up = cos_lat*cos_lon*dx + cos_lat*sin_lon*dy + sin_lat*dz; + + double range_km = sqrt(dx*dx + dy*dy + dz*dz); + double el = asin(up / range_km); + double az = atan2(east, -south); + if (az < 0.0) az += 2.0 * M_PI; + + /* Inverse: topocentric → ECEF */ + double sat_recovered[3]; + od_topocentric_to_ecef(az, el, range_km, obs_ecef, + obs_lat, obs_lon, sat_recovered); + + snprintf(msg, sizeof(msg), "%s pos X", cases[i].name); + CLOSE(sat_recovered[0], sat[0], 1e-6, msg); + snprintf(msg, sizeof(msg), "%s pos Y", cases[i].name); + CLOSE(sat_recovered[1], sat[1], 1e-6, msg); + snprintf(msg, sizeof(msg), "%s pos Z", cases[i].name); + CLOSE(sat_recovered[2], sat[2], 1e-6, msg); + } +} + + +/* ── Test: full pipeline ECI → topo → ECEF → TEME ─────── */ + +static void +test_full_inverse_pipeline(void) +{ + fprintf(stderr, "\n--- Full inverse pipeline ---\n"); + + double jd = 2460000.5; /* arbitrary epoch */ + double gmst = od_gmst_from_jd(jd); + + /* Start with TEME state */ + double pos_teme[3] = {-4000.0, 4000.0, 3000.0}; + double vel_teme[3] = {-4.0, -5.0, 3.0}; + + /* Forward: TEME → ECEF */ + double cos_g = cos(gmst), sin_g = sin(gmst); + double pos_ecef[3], vel_ecef[3]; + double omega = 7.2921158553e-5; + + pos_ecef[0] = cos_g * pos_teme[0] + sin_g * pos_teme[1]; + pos_ecef[1] = -sin_g * pos_teme[0] + cos_g * pos_teme[1]; + pos_ecef[2] = pos_teme[2]; + + vel_ecef[0] = cos_g * vel_teme[0] + sin_g * vel_teme[1] + omega * pos_ecef[1]; + vel_ecef[1] = -sin_g * vel_teme[0] + cos_g * vel_teme[1] - omega * pos_ecef[0]; + vel_ecef[2] = vel_teme[2]; + + /* ECEF → topocentric (observer at origin-ish, 0 lat 0 lon) */ + double obs_lat = 0.0, obs_lon = 0.0; + double obs_ecef[3]; + od_observer_to_ecef(obs_lat, obs_lon, 0.0, obs_ecef); + + double dx = pos_ecef[0] - obs_ecef[0]; + double dy = pos_ecef[1] - obs_ecef[1]; + double dz = pos_ecef[2] - obs_ecef[2]; + + double sin_lat = sin(obs_lat), cos_lat = cos(obs_lat); + double sin_lon = sin(obs_lon), cos_lon = cos(obs_lon); + + double south = sin_lat*cos_lon*dx + sin_lat*sin_lon*dy - cos_lat*dz; + double east = -sin_lon*dx + cos_lon*dy; + double up = cos_lat*cos_lon*dx + cos_lat*sin_lon*dy + sin_lat*dz; + + double range_km = sqrt(dx*dx + dy*dy + dz*dz); + double el = asin(up / range_km); + double az = atan2(east, -south); + if (az < 0.0) az += 2.0 * M_PI; + + /* Inverse pipeline: topo → ECEF → TEME */ + double sat_ecef_inv[3]; + od_topocentric_to_ecef(az, el, range_km, obs_ecef, + obs_lat, obs_lon, sat_ecef_inv); + + double pos_teme_inv[3]; + od_ecef_to_teme(sat_ecef_inv, NULL, gmst, pos_teme_inv, NULL); + + CLOSE(pos_teme_inv[0], pos_teme[0], 1e-6, "pipeline TEME X recovery"); + CLOSE(pos_teme_inv[1], pos_teme[1], 1e-6, "pipeline TEME Y recovery"); + CLOSE(pos_teme_inv[2], pos_teme[2], 1e-6, "pipeline TEME Z recovery"); + + /* Also verify velocity through full inverse */ + double pos_teme_inv2[3], vel_teme_inv[3]; + od_ecef_to_teme(pos_ecef, vel_ecef, gmst, pos_teme_inv2, vel_teme_inv); + + CLOSE(vel_teme_inv[0], vel_teme[0], 1e-9, "pipeline vel X recovery"); + CLOSE(vel_teme_inv[1], vel_teme[1], 1e-9, "pipeline vel Y recovery"); + CLOSE(vel_teme_inv[2], vel_teme[2], 1e-9, "pipeline vel Z recovery"); +} + + +/* ── Main ──────────────────────────────────────────────── */ + +int +main(void) +{ + fprintf(stderr, "pg_orrery OD math unit tests\n"); + fprintf(stderr, "============================\n"); + + test_eci_keplerian_roundtrip(); + test_keplerian_equinoctial_roundtrip(); + test_brouwer_kozai_inverse(); + test_ecef_teme_inverse(); + test_topocentric_ecef_inverse(); + test_full_inverse_pipeline(); + + fprintf(stderr, "\n%d/%d tests passed.\n", n_pass, n_run); + + return (n_pass == n_run) ? 0 : 1; +}