diff --git a/.gitignore b/.gitignore index 365916c..3acc687 100644 --- a/.gitignore +++ b/.gitignore @@ -22,6 +22,7 @@ log/ test/matrix-logs/ test/test_de_reader test/test_od_math +test/test_od_iod # Docs site docs/node_modules/ diff --git a/Makefile b/Makefile index 689e024..936bce8 100644 --- a/Makefile +++ b/Makefile @@ -15,7 +15,7 @@ OBJS = src/pg_orrery.o src/tle_type.o src/eci_type.o src/observer_type.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/od_math.o src/od_solver.o src/od_funcs.o + src/od_math.o src/od_iod.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 @@ -61,6 +61,14 @@ test-od-math: test/test_od_math.c src/od_math.c src/od_math.h .PHONY: test-od-math +# ── Standalone IOD unit test (no PostgreSQL dependency) ── +# Gibbs method: 3-position orbit recovery, coplanarity checks. +test-od-iod: test/test_od_iod.c src/od_iod.c src/od_iod.h src/od_math.c src/od_math.h + $(CC) -Wall -Werror -Isrc -o test/test_od_iod $< src/od_iod.c src/od_math.c -lm + ./test/test_od_iod + +.PHONY: test-od-iod + # ── PG version test matrix ───────────────────────────────── PG_TEST_VERSIONS ?= 14 15 16 17 18 diff --git a/src/od_funcs.c b/src/od_funcs.c index 76dcd64..2403ed2 100644 --- a/src/od_funcs.c +++ b/src/od_funcs.c @@ -288,17 +288,12 @@ tle_from_topocentric(PG_FUNCTION_ARGS) config.observers = &observer; config.n_observers = 1; - /* 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); + if (has_seed) + 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); + rc = od_fit_tle(obs, n_topo, has_seed ? &seed_sat : NULL, &config, &result); pfree(obs); @@ -444,17 +439,12 @@ tle_from_topocentric_multi(PG_FUNCTION_ARGS) config.observers = observers; config.n_observers = n_obs_sites; - /* 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); + if (has_seed) + 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); + rc = od_fit_tle(obs, n_topo, has_seed ? &seed_sat : NULL, &config, &result); pfree(obs); pfree(observers); diff --git a/src/od_iod.c b/src/od_iod.c new file mode 100644 index 0000000..4ba44d1 --- /dev/null +++ b/src/od_iod.c @@ -0,0 +1,127 @@ +/* + * od_iod.c -- Gibbs method for initial orbit determination + * + * Given 3 position vectors, computes the velocity at the middle + * observation via vector cross-product geometry (Vallado Algorithm 54). + * + * The Gibbs method requires 3 coplanar position vectors (which all + * orbit positions are, modulo perturbations and measurement noise). + * It avoids iteration entirely -- pure linear algebra. + * + * Reference: Vallado, "Fundamentals of Astrodynamics", 4th ed., p.459 + */ + +#include "od_iod.h" +#include + +/* WGS-72 gravitational parameter -- must match SGP4 */ +#define MU_KM3_S2 398600.8 + +/* Coplanarity tolerance: cos(angle) between r1 and (r2 x r3) plane. + * 0.05 corresponds to about 3 degrees, generous for noisy obs. */ +#define COPLANAR_TOL 0.05 + +static double +vec_mag(const double v[3]) +{ + return sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]); +} + +static void +vec_cross(const double a[3], const double b[3], double out[3]) +{ + out[0] = a[1]*b[2] - a[2]*b[1]; + out[1] = a[2]*b[0] - a[0]*b[2]; + out[2] = a[0]*b[1] - a[1]*b[0]; +} + +static double +vec_dot(const double a[3], const double b[3]) +{ + return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]; +} + +int +od_gibbs(const double pos1[3], const double pos2[3], + const double pos3[3], + double jd1, double jd2, double jd3, + od_iod_result_t *result) +{ + double r1_mag, r2_mag, r3_mag; + double z12[3], z23[3], z31[3]; + double z23_mag; + double coplanar; + double D[3], N[3], S[3]; + double D_mag, N_mag; + double v2[3]; + double v2_coeff; + double D_cross_r2[3]; + int i, rc; + + result->valid = 0; + + r1_mag = vec_mag(pos1); + r2_mag = vec_mag(pos2); + r3_mag = vec_mag(pos3); + + if (r1_mag < 1.0 || r2_mag < 1.0 || r3_mag < 1.0) + return -1; + + /* Cross products */ + vec_cross(pos1, pos2, z12); + vec_cross(pos2, pos3, z23); + vec_cross(pos3, pos1, z31); + + /* Coplanarity check: |r1 . z23| / (|r1| * |z23|) should be small */ + z23_mag = vec_mag(z23); + if (z23_mag < 1e-10) + return -1; /* pos2 and pos3 are parallel (same direction) */ + + coplanar = fabs(vec_dot(pos1, z23)) / (r1_mag * z23_mag); + if (coplanar > COPLANAR_TOL) + return -1; + + /* D = z12 + z23 + z31 */ + for (i = 0; i < 3; i++) + D[i] = z12[i] + z23[i] + z31[i]; + + /* N = |r1| * z23 + |r2| * z31 + |r3| * z12 */ + for (i = 0; i < 3; i++) + N[i] = r1_mag * z23[i] + r2_mag * z31[i] + r3_mag * z12[i]; + + /* S = (|r2| - |r3|) * r1 + (|r3| - |r1|) * r2 + (|r1| - |r2|) * r3 */ + for (i = 0; i < 3; i++) + S[i] = (r2_mag - r3_mag) * pos1[i] + + (r3_mag - r1_mag) * pos2[i] + + (r1_mag - r2_mag) * pos3[i]; + + D_mag = vec_mag(D); + N_mag = vec_mag(N); + + if (D_mag < 1e-10 || N_mag < 1e-10) + return -1; + + /* v2 = sqrt(mu / (N_mag * D_mag)) * (D x r2 / |r2| + S) */ + v2_coeff = sqrt(MU_KM3_S2 / (N_mag * D_mag)); + + vec_cross(D, pos2, D_cross_r2); + + for (i = 0; i < 3; i++) + v2[i] = v2_coeff * (D_cross_r2[i] / r2_mag + S[i]); + + /* Convert km/s velocity to the form expected by od_eci_to_keplerian */ + rc = od_eci_to_keplerian(pos2, v2, &result->kep); + if (rc != 0) + return -1; + + /* Sanity: eccentricity < 1 and positive mean motion */ + if (result->kep.ecc >= 1.0 || result->kep.n <= 0.0) + return -1; + + result->epoch_jd = jd2; + result->valid = 1; + + (void)jd1; + (void)jd3; + return 0; +} diff --git a/src/od_iod.h b/src/od_iod.h new file mode 100644 index 0000000..eba18d2 --- /dev/null +++ b/src/od_iod.h @@ -0,0 +1,45 @@ +/* + * od_iod.h -- Initial orbit determination (Gibbs method) + * + * Given 3 position vectors and their times, recover a velocity + * at the middle observation using Gibbs' vector cross-product + * approach (Vallado Algorithm 54). The resulting (r, v) pair + * provides a seed orbit for the DC solver. + * + * Uses WGS-72 mu (398600.8 km^3/s^2) for SGP4 consistency. + */ + +#ifndef PG_ORRERY_OD_IOD_H +#define PG_ORRERY_OD_IOD_H + +#include "od_math.h" + +/* + * IOD result: Keplerian elements at the middle observation epoch. + */ +typedef struct +{ + od_keplerian_t kep; + double epoch_jd; + int valid; /* 1 if physically valid orbit */ +} od_iod_result_t; + +/* + * Gibbs method: recover velocity at r2 from 3 coplanar positions. + * + * pos1, pos2, pos3: TEME position vectors (km) + * jd1, jd2, jd3: Julian dates of each observation + * result: output Keplerian elements at epoch jd2 + * + * Returns 0 on success, -1 if vectors are not coplanar (within + * tolerance) or orbit is degenerate. + * + * Coplanarity check: |r1 . (r2 x r3)| / (|r1| * |r2 x r3|) < 0.05 + * (about 3 degrees -- generous to handle noise). + */ +int od_gibbs(const double pos1[3], const double pos2[3], + const double pos3[3], + double jd1, double jd2, double jd3, + od_iod_result_t *result); + +#endif /* PG_ORRERY_OD_IOD_H */ diff --git a/src/od_solver.c b/src/od_solver.c index 69efad5..49b6381 100644 --- a/src/od_solver.c +++ b/src/od_solver.c @@ -22,6 +22,7 @@ #include "od_solver.h" #include "od_math.h" +#include "od_iod.h" #include "norad.h" #include "norad_in.h" @@ -421,43 +422,122 @@ enforce_bounds(od_equinoctial_t *eq) /* ── 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). + * Helper: populate a TLE from Keplerian elements and an epoch. + */ +static void +kep_to_seed_tle(const od_keplerian_t *kep, double epoch_jd, tle_t *guess) +{ + memset(guess, 0, sizeof(tle_t)); + guess->epoch = epoch_jd; + 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'; +} + +/* + * Compute initial orbit from ECI observations. + * + * ECI observations include full velocity, so single-obs r,v→elements + * is exact for two-body and typically superior to Gibbs (which only + * uses positions). Gibbs is only attempted as fallback when the + * single-obs conversion fails (degenerate orbit). */ 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; + /* Primary: use first observation's position/velocity (exact for + * two-body, excellent for short-arc LEO) */ + if (od_eci_to_keplerian(obs[0].data, obs[0].data + 3, &kep) == 0) + { + kep_to_seed_tle(&kep, obs[0].jd, guess); + return; + } + + /* Fallback: Gibbs from 3 well-spaced positions */ + if (n_obs >= 3) + { + int i0 = 0, i1 = n_obs / 2, i2 = n_obs - 1; + od_iod_result_t iod; + + if (od_gibbs(obs[i0].data, obs[i1].data, obs[i2].data, + obs[i0].jd, obs[i1].jd, obs[i2].jd, + &iod) == 0) + { + kep_to_seed_tle(&iod.kep, iod.epoch_jd, guess); + return; + } + } + + /* Last resort: zero elements (solver will struggle) */ 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; +/* + * Compute initial orbit from topocentric observations. + * + * Converts 3 well-spaced topo observations to TEME positions + * via topo→ECEF→TEME, then uses Gibbs to recover the orbit. + * Returns 0 on success, -1 if Gibbs fails. + */ +static int +initial_guess_from_topo(const od_observation_t *obs, int n_obs, + const od_observer_t *observers, + tle_t *guess) +{ + int idx[3]; + double pos_teme[3][3]; + double jd[3]; + int k; + od_iod_result_t iod; + + if (n_obs < 3) + return -1; + + idx[0] = 0; + idx[1] = n_obs / 2; + idx[2] = n_obs - 1; + + for (k = 0; k < 3; k++) + { + const od_observation_t *o = &obs[idx[k]]; + const od_observer_t *observer = &observers[o->observer_idx]; + double sat_ecef[3]; + double gmst; + double vel_zero[3] = {0, 0, 0}; + + /* Topo → ECEF */ + od_topocentric_to_ecef(o->data[0], o->data[1], o->data[2], + observer->ecef, observer->lat, observer->lon, + sat_ecef); + + /* ECEF → TEME */ + gmst = od_gmst_from_jd(o->jd); + od_ecef_to_teme(sat_ecef, vel_zero, gmst, + pos_teme[k], vel_zero); + + jd[k] = o->jd; + } + + if (od_gibbs(pos_teme[0], pos_teme[1], pos_teme[2], + jd[0], jd[1], jd[2], &iod) != 0) + return -1; + + kep_to_seed_tle(&iod.kep, iod.epoch_jd, guess); + return 0; } @@ -509,13 +589,20 @@ od_fit_tle(const od_observation_t *obs, int n_obs, } else { - if (config->obs_type != OD_OBS_ECI) + if (config->obs_type == OD_OBS_ECI) + initial_guess_from_eci(obs, n_obs, &seed_tle); + else { - snprintf(result->status, sizeof(result->status), - "seed TLE required for topocentric fitting"); - return -1; + int rc_iod = initial_guess_from_topo(obs, n_obs, + config->observers, + &seed_tle); + if (rc_iod != 0) + { + snprintf(result->status, sizeof(result->status), + "IOD bootstrap failed (Gibbs): need seed TLE"); + return -1; + } } - initial_guess_from_eci(obs, n_obs, &seed_tle); } /* Extract equinoctial elements from seed */ diff --git a/test/expected/od_fit.out b/test/expected/od_fit.out index de4d409..9d0ac61 100644 --- a/test/expected/od_fit.out +++ b/test/expected/od_fit.out @@ -412,3 +412,73 @@ EXCEPTION RAISE NOTICE 'OK: observer_id out of range error caught'; END $$; NOTICE: OK: observer_id out of range error caught +-- ============================================================ +-- Test 12: Topocentric without seed TLE (IOD bootstrap) +-- +-- The Gibbs method provides an initial orbit from 3 well-spaced +-- topocentric observations, eliminating the seed requirement. +-- Wider tolerance (~20 km) because the IOD initial guess is rough. +-- ============================================================ +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)).* + FROM topo_obs, mit +) +SELECT + rms_final < 20.0 AS rms_under_20km, + status IN ('converged', 'max_iterations') AS solver_ran +FROM result; + rms_under_20km | solver_ran +----------------+------------ + t | t +(1 row) + +-- ============================================================ +-- Test 13: ECI without seed, Gibbs-improved initial guess +-- +-- With Gibbs, the ECI seedless path uses 3-position geometry +-- instead of a single r,v pair. Should converge. +-- ============================================================ +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)).* FROM obs +) +SELECT + iterations > 0 AS has_iterations, + rms_final < 5.0 AS rms_under_5km, + status = 'converged' AS did_converge +FROM result; + has_iterations | rms_under_5km | did_converge +----------------+---------------+-------------- + t | t | t +(1 row) + diff --git a/test/sql/od_fit.sql b/test/sql/od_fit.sql index 882b218..d6f5261 100644 --- a/test/sql/od_fit.sql +++ b/test/sql/od_fit.sql @@ -388,3 +388,67 @@ EXCEPTION WHEN invalid_parameter_value THEN RAISE NOTICE 'OK: observer_id out of range error caught'; END $$; + +-- ============================================================ +-- Test 12: Topocentric without seed TLE (IOD bootstrap) +-- +-- The Gibbs method provides an initial orbit from 3 well-spaced +-- topocentric observations, eliminating the seed requirement. +-- Wider tolerance (~20 km) because the IOD initial guess is rough. +-- ============================================================ + +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)).* + FROM topo_obs, mit +) +SELECT + rms_final < 20.0 AS rms_under_20km, + status IN ('converged', 'max_iterations') AS solver_ran +FROM result; + +-- ============================================================ +-- Test 13: ECI without seed, Gibbs-improved initial guess +-- +-- With Gibbs, the ECI seedless path uses 3-position geometry +-- instead of a single r,v pair. Should converge. +-- ============================================================ + +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)).* FROM obs +) +SELECT + iterations > 0 AS has_iterations, + rms_final < 5.0 AS rms_under_5km, + status = 'converged' AS did_converge +FROM result; diff --git a/test/test_od_iod.c b/test/test_od_iod.c new file mode 100644 index 0000000..5324695 --- /dev/null +++ b/test/test_od_iod.c @@ -0,0 +1,220 @@ +/* + * test_od_iod.c -- Standalone unit tests for Gibbs IOD + * + * No PostgreSQL dependency. Exercises: + * - ISS-like 3-position Gibbs recovery + * - GEO-like orbit with wide time separation + * - Coplanar failure detection + * - Near-circular orbit + * + * Build: cc -Wall -Werror -Isrc -o test/test_od_iod \ + * test/test_od_iod.c src/od_iod.c src/od_math.c -lm + * Run: ./test/test_od_iod + */ + +#include "od_iod.h" +#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: ISS-like orbit ──────────────────────────────── */ + +static void +test_gibbs_iss(void) +{ + /* ISS-like orbit: 408 km altitude, 51.6 deg inclination. + * Generate 3 positions from known Keplerian elements. */ + od_keplerian_t kep; + double pos1[3], vel1[3], pos2[3], vel2[3], pos3[3], vel3[3]; + od_iod_result_t result; + int rc; + double n_rad_s, period_s, dt; + + fprintf(stderr, "\n--- Gibbs: ISS-like orbit ---\n"); + + kep.n = 0.001127; /* ~15.5 rev/day in rad/min */ + kep.ecc = 0.0007; + kep.inc = 0.9012; /* ~51.6 deg */ + kep.raan = 3.0; + kep.argp = 0.5; + kep.M = 0.0; + kep.bstar = 0.0; + + /* Period in seconds, time step = period/6 */ + n_rad_s = kep.n / 60.0; + period_s = 2.0 * M_PI / n_rad_s; + dt = period_s / 6.0; /* ~15 min */ + + od_keplerian_to_eci(&kep, pos1, vel1); + + /* Advance M by dt */ + kep.M = od_normalize_angle(kep.M + kep.n * (dt / 60.0)); + od_keplerian_to_eci(&kep, pos2, vel2); + + kep.M = od_normalize_angle(kep.M + kep.n * (dt / 60.0)); + od_keplerian_to_eci(&kep, pos3, vel3); + + /* Julian dates (arbitrary epoch + dt in days) */ + rc = od_gibbs(pos1, pos2, pos3, + 2451545.0, + 2451545.0 + dt / 86400.0, + 2451545.0 + 2.0 * dt / 86400.0, + &result); + + RUN(rc == 0, "Gibbs ISS returns success"); + RUN(result.valid == 1, "result is valid"); + CLOSE(result.kep.ecc, 0.0007, 0.01, "eccentricity recovered"); + CLOSE(result.kep.inc, 0.9012, 0.02, "inclination recovered"); + RUN(result.kep.n > 0.0, "positive mean motion"); + CLOSE(result.kep.n, 0.001127, 0.0001, "mean motion recovered"); +} + + +/* ── Test: GEO-like orbit ──────────────────────────────── */ + +static void +test_gibbs_geo(void) +{ + od_keplerian_t kep; + double pos1[3], vel1[3], pos2[3], vel2[3], pos3[3], vel3[3]; + od_iod_result_t result; + int rc; + double dt; + + fprintf(stderr, "\n--- Gibbs: GEO-like orbit ---\n"); + + kep.n = 0.0000729; /* ~1 rev/day */ + kep.ecc = 0.0001; + kep.inc = 0.001; /* near-equatorial */ + kep.raan = 0.0; + kep.argp = 0.0; + kep.M = 0.0; + kep.bstar = 0.0; + + /* 2 hours between observations */ + dt = 7200.0; + + od_keplerian_to_eci(&kep, pos1, vel1); + + kep.M = od_normalize_angle(kep.M + kep.n * (dt / 60.0)); + od_keplerian_to_eci(&kep, pos2, vel2); + + kep.M = od_normalize_angle(kep.M + kep.n * (dt / 60.0)); + od_keplerian_to_eci(&kep, pos3, vel3); + + rc = od_gibbs(pos1, pos2, pos3, + 2451545.0, + 2451545.0 + dt / 86400.0, + 2451545.0 + 2.0 * dt / 86400.0, + &result); + + RUN(rc == 0, "Gibbs GEO returns success"); + RUN(result.valid == 1, "result is valid"); + RUN(result.kep.ecc < 0.01, "near-circular eccentricity"); + CLOSE(result.kep.n, 0.0000729, 0.00001, "GEO mean motion recovered"); +} + + +/* ── Test: coplanar failure ────────────────────────────── */ + +static void +test_gibbs_coplanar_fail(void) +{ + /* Three vectors that are NOT coplanar (large out-of-plane angle) */ + double pos1[3] = {6778.0, 0.0, 0.0}; + double pos2[3] = {0.0, 6778.0, 0.0}; + double pos3[3] = {0.0, 0.0, 6778.0}; + od_iod_result_t result; + int rc; + + fprintf(stderr, "\n--- Gibbs: coplanarity failure ---\n"); + + rc = od_gibbs(pos1, pos2, pos3, + 2451545.0, 2451545.01, 2451545.02, + &result); + + RUN(rc != 0, "non-coplanar vectors rejected"); + RUN(result.valid == 0, "result marked invalid"); +} + + +/* ── Test: near-circular orbit ─────────────────────────── */ + +static void +test_gibbs_circular(void) +{ + od_keplerian_t kep; + double pos1[3], vel1[3], pos2[3], vel2[3], pos3[3], vel3[3]; + od_iod_result_t result; + int rc; + double dt; + + fprintf(stderr, "\n--- Gibbs: near-circular orbit ---\n"); + + kep.n = 0.001127; + kep.ecc = 0.00001; /* nearly perfect circle */ + kep.inc = 0.5; + kep.raan = 1.0; + kep.argp = 0.0; + kep.M = 0.0; + kep.bstar = 0.0; + + dt = 900.0; /* 15 min */ + + od_keplerian_to_eci(&kep, pos1, vel1); + + kep.M = od_normalize_angle(kep.M + kep.n * (dt / 60.0)); + od_keplerian_to_eci(&kep, pos2, vel2); + + kep.M = od_normalize_angle(kep.M + kep.n * (dt / 60.0)); + od_keplerian_to_eci(&kep, pos3, vel3); + + rc = od_gibbs(pos1, pos2, pos3, + 2451545.0, + 2451545.0 + dt / 86400.0, + 2451545.0 + 2.0 * dt / 86400.0, + &result); + + RUN(rc == 0, "Gibbs circular returns success"); + RUN(result.kep.ecc < 0.001, "eccentricity near zero"); + CLOSE(result.kep.n, 0.001127, 0.0001, "mean motion recovered"); +} + + +int +main(void) +{ + fprintf(stderr, "pg_orrery IOD unit tests\n"); + fprintf(stderr, "========================\n"); + + test_gibbs_iss(); + test_gibbs_geo(); + test_gibbs_coplanar_fail(); + test_gibbs_circular(); + + fprintf(stderr, "\n%d/%d tests passed.\n", n_pass, n_run); + return (n_pass == n_run) ? 0 : 1; +}