Ryan Malloy
adfb6949e1
Range rate: topocentric residuals now include an optional 4th component (dot(Δr, v_ecef) / |Δr|) with OD_RR_SCALE=10.0 for unit balancing. Controlled via fit_range_rate parameter on tle_from_topocentric(). Weighted observations: per-observation weights applied as √w scaling to both residuals and Jacobian rows, producing the weighted normal equations H'WH without explicit W construction. Weights parameter added to tle_from_eci, tle_from_topocentric, and tle_from_angles. Gauss angles-only IOD: Vallado Algorithm 52 implementation for seed-free orbit recovery from 3+ RA/Dec observations. New RA/Dec residual function with cos(dec) scaling and wrap-around handling. New tle_from_angles() and tle_from_angles_multi() SQL functions accepting RA in hours [0,24), Dec in degrees [-90,90]. New standalone test suite: test_od_gauss (17 assertions). New regression tests: Tests 18-25 covering range rate, weights, angles-only with/without seed, and error cases.
158 lines
5.3 KiB
C
158 lines
5.3 KiB
C
/*
|
|
* 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 <math.h>
|
|
|
|
/*
|
|
* 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);
|
|
|
|
/* ── RA/Dec utilities (angles-only mode) ───────────────── */
|
|
|
|
/*
|
|
* RA/Dec (radians) → unit line-of-sight vector (equatorial TEME).
|
|
*/
|
|
void od_radec_to_los(double ra, double dec, double los[3]);
|
|
|
|
/*
|
|
* TEME satellite pos + observer ECEF + GMST → RA/Dec (radians).
|
|
* Computes observer-relative direction in inertial (TEME) frame.
|
|
* TEME ≈ J2000 equatorial for our accuracy needs (~1 arcsec offset
|
|
* from nutation, well below TLE accuracy floor of ~1 km ≈ 20 arcsec at LEO).
|
|
*/
|
|
void od_teme_to_radec(const double pos_teme[3], const double obs_ecef[3],
|
|
double gmst, double *ra, double *dec);
|
|
|
|
/*
|
|
* 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 */
|