Ryan Malloy
6e57071970
Eliminates the seed TLE requirement for topocentric fitting by computing an initial orbit estimate from 3 well-spaced observations using the Gibbs method. ECI fitting retains the single-observation r,v approach (exact for two-body) with Gibbs as fallback.
128 lines
3.3 KiB
C
128 lines
3.3 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;
|
|
}
|