Ryan Malloy
87ab81e7d0
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.
514 lines
15 KiB
C
514 lines
15 KiB
C
/*
|
|
* 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;
|
|
}
|