/* * 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; } /* ================================================================ * 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; }