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.
312 lines
9.2 KiB
C
312 lines
9.2 KiB
C
/*
|
|
* 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 <math.h>
|
|
|
|
/* 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;
|
|
}
|
|
|
|
|
|
/* ================================================================
|
|
* Gauss method for angles-only initial orbit determination
|
|
*
|
|
* Given 3 RA/Dec observations and observer positions, recovers the
|
|
* orbit at the middle observation epoch. Vallado Algorithm 52.
|
|
*
|
|
* Steps:
|
|
* 1. Compute LOS unit vectors from RA/Dec
|
|
* 2. Rotate observer ECEF → TEME at each epoch via GMST
|
|
* 3. Build D matrix from LOS vectors and observer positions
|
|
* 4. Iteratively solve for slant range at middle observation
|
|
* 5. Use Gibbs/Herrick-Gibbs to get velocity at r2
|
|
* 6. Convert (r2, v2) → Keplerian elements
|
|
* ================================================================
|
|
*/
|
|
|
|
int
|
|
od_gauss(const double ra[3], const double dec[3],
|
|
const double jd[3],
|
|
const double obs_ecef[3][3],
|
|
od_iod_result_t *result)
|
|
{
|
|
double L[3][3]; /* LOS unit vectors */
|
|
double R[3][3]; /* Observer positions in TEME */
|
|
double tau1, tau3, tau;
|
|
double D[3][3]; /* D matrix */
|
|
double D0;
|
|
double A, B;
|
|
double r2_mag, r2_mag_old;
|
|
double rho[3]; /* slant ranges */
|
|
double r2[3], v2[3];
|
|
int iter, i, rc;
|
|
double gmst;
|
|
|
|
result->valid = 0;
|
|
|
|
/* Time intervals in seconds */
|
|
tau1 = (jd[0] - jd[1]) * 86400.0;
|
|
tau3 = (jd[2] - jd[1]) * 86400.0;
|
|
tau = tau3 - tau1;
|
|
|
|
if (fabs(tau) < 1.0)
|
|
return -1; /* observations too close in time */
|
|
|
|
/* LOS unit vectors from RA/Dec */
|
|
for (i = 0; i < 3; i++)
|
|
od_radec_to_los(ra[i], dec[i], L[i]);
|
|
|
|
/* Observer ECEF → TEME */
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
double vel_zero[3] = {0, 0, 0};
|
|
double vel_dummy[3];
|
|
gmst = od_gmst_from_jd(jd[i]);
|
|
od_ecef_to_teme(obs_ecef[i], vel_zero, gmst, R[i], vel_dummy);
|
|
}
|
|
|
|
/* Build D matrix: D[i][j] = L[i] . (L_cross products) */
|
|
{
|
|
double L2xL3[3], L1xL3[3], L1xL2[3];
|
|
|
|
vec_cross(L[1], L[2], L2xL3);
|
|
vec_cross(L[0], L[2], L1xL3);
|
|
vec_cross(L[0], L[1], L1xL2);
|
|
|
|
D0 = vec_dot(L[0], L2xL3);
|
|
|
|
if (fabs(D0) < 1e-15)
|
|
return -1; /* coplanar LOS vectors */
|
|
|
|
/* D matrix: each row is dot products of R[i] with cross products */
|
|
D[0][0] = vec_dot(R[0], L2xL3);
|
|
D[0][1] = vec_dot(R[0], L1xL3);
|
|
D[0][2] = vec_dot(R[0], L1xL2);
|
|
|
|
D[1][0] = vec_dot(R[1], L2xL3);
|
|
D[1][1] = vec_dot(R[1], L1xL3);
|
|
D[1][2] = vec_dot(R[1], L1xL2);
|
|
|
|
D[2][0] = vec_dot(R[2], L2xL3);
|
|
D[2][1] = vec_dot(R[2], L1xL3);
|
|
D[2][2] = vec_dot(R[2], L1xL2);
|
|
}
|
|
|
|
/* A and B coefficients for the range equation */
|
|
A = (-D[0][1] * tau3 / tau + D[1][1] + D[2][1] * tau1 / tau) / D0;
|
|
B = (D[0][1] * (tau3 * tau3 - tau * tau) * tau3 / tau
|
|
+ D[2][1] * (tau * tau - tau1 * tau1) * tau1 / tau) / (6.0 * D0);
|
|
|
|
/* Initial guess: r2_mag from observer position magnitude */
|
|
r2_mag = vec_mag(R[1]) + 500.0; /* rough LEO assumption */
|
|
|
|
/* Iterate to find r2_mag */
|
|
for (iter = 0; iter < 50; iter++)
|
|
{
|
|
double r2_3 = r2_mag * r2_mag * r2_mag;
|
|
double f1, f3, g1, g3;
|
|
double c1, c3;
|
|
|
|
/* f and g series (two-body, truncated) */
|
|
f1 = 1.0 - 0.5 * MU_KM3_S2 * tau1 * tau1 / r2_3;
|
|
f3 = 1.0 - 0.5 * MU_KM3_S2 * tau3 * tau3 / r2_3;
|
|
g1 = tau1 - MU_KM3_S2 * tau1 * tau1 * tau1 / (6.0 * r2_3);
|
|
g3 = tau3 - MU_KM3_S2 * tau3 * tau3 * tau3 / (6.0 * r2_3);
|
|
|
|
/* Lagrange coefficients */
|
|
c1 = g3 / (f1 * g3 - f3 * g1);
|
|
c3 = -g1 / (f1 * g3 - f3 * g1);
|
|
|
|
/* Slant ranges */
|
|
rho[0] = (-D[0][0] + D[1][0] / c1 - D[2][0] * c3 / c1) / D0;
|
|
rho[1] = A + B / r2_3;
|
|
rho[2] = (-D[0][2] * c1 / c3 + D[1][2] / c3 - D[2][2]) / D0;
|
|
|
|
/* Position at middle observation */
|
|
for (i = 0; i < 3; i++)
|
|
r2[i] = R[1][i] + rho[1] * L[1][i];
|
|
|
|
r2_mag_old = r2_mag;
|
|
r2_mag = vec_mag(r2);
|
|
|
|
if (fabs(r2_mag - r2_mag_old) < 1e-6)
|
|
break;
|
|
}
|
|
|
|
if (iter >= 50 || r2_mag < 100.0)
|
|
return -1; /* failed to converge or nonsensical result */
|
|
|
|
/* Check slant ranges are positive */
|
|
if (rho[0] < 0.0 || rho[1] < 0.0 || rho[2] < 0.0)
|
|
return -1;
|
|
|
|
/* Compute all 3 position vectors for Gibbs */
|
|
{
|
|
double pos1[3], pos3[3];
|
|
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
pos1[i] = R[0][i] + rho[0] * L[0][i];
|
|
pos3[i] = R[2][i] + rho[2] * L[2][i];
|
|
}
|
|
|
|
/* Use Gibbs to get velocity at r2 */
|
|
rc = od_gibbs(pos1, r2, pos3, jd[0], jd[1], jd[2], result);
|
|
|
|
/* If Gibbs fails (short arc / nearly coplanar), try Herrick-Gibbs */
|
|
if (rc != 0)
|
|
{
|
|
/* Herrick-Gibbs: use f and g series for velocity estimation */
|
|
double dt31 = (jd[2] - jd[0]) * 86400.0;
|
|
double dt21 = (jd[1] - jd[0]) * 86400.0;
|
|
double dt32 = (jd[2] - jd[1]) * 86400.0;
|
|
|
|
if (fabs(dt31) < 1.0 || fabs(dt21) < 1.0 || fabs(dt32) < 1.0)
|
|
return -1;
|
|
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
v2[i] = -dt32 * (1.0 / (dt21 * dt31)
|
|
+ MU_KM3_S2 / (12.0 * vec_mag(pos1) *
|
|
vec_mag(pos1) * vec_mag(pos1))) * pos1[i]
|
|
+ (dt32 - dt21) * (1.0 / (dt21 * dt32)
|
|
+ MU_KM3_S2 / (12.0 * r2_mag * r2_mag * r2_mag)) * r2[i]
|
|
+ dt21 * (1.0 / (dt32 * dt31)
|
|
+ MU_KM3_S2 / (12.0 * vec_mag(pos3) *
|
|
vec_mag(pos3) * vec_mag(pos3))) * pos3[i];
|
|
}
|
|
|
|
rc = od_eci_to_keplerian(r2, v2, &result->kep);
|
|
if (rc != 0)
|
|
return -1;
|
|
|
|
if (result->kep.ecc >= 1.0 || result->kep.n <= 0.0)
|
|
return -1;
|
|
|
|
result->epoch_jd = jd[1];
|
|
result->valid = 1;
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|