pg_orrery/test/test_od_math.c
Ryan Malloy 87ab81e7d0 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.
2026-02-17 15:44:48 -07:00

465 lines
16 KiB
C

/*
* 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 <stdio.h>
#include <stdlib.h>
#include <math.h>
/* ── 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;
}