Ryan Malloy
bca8b3e7eb
Computes formal covariance (H^T·H)^{-1} via LAPACK dpotrf_/dpotri_
after DC convergence. Returns upper-triangle array (21 elements for
6-state, 28 for 7-state with B*), condition number from SVD, and
nstate count. Covariance is computed even for perfect-seed fits.
Bumps extension to v0.5.0 with full install SQL and migration path.
1013 lines
32 KiB
C
1013 lines
32 KiB
C
/*
|
|
* od_solver.c -- Batch weighted least-squares differential correction
|
|
*
|
|
* Implements TLE fitting from observations per Vallado & Crawford (2008)
|
|
* AIAA 2008-6770. Uses equinoctial elements to avoid singularities,
|
|
* SGP4/SDP4 as the propagation engine, and LAPACK dgelss_() for SVD.
|
|
*
|
|
* Algorithm summary:
|
|
* 1. Convert seed TLE to equinoctial elements
|
|
* 2. For each iteration:
|
|
* a. Propagate nominal state to all observation times
|
|
* b. Compute residuals (observed - computed)
|
|
* c. Build Jacobian by finite-differencing each element
|
|
* d. Solve min ||Ax - b|| via SVD
|
|
* e. Apply step limiting (Vallado 2008 Section V.B)
|
|
* f. Check convergence
|
|
* 3. Convert final equinoctial → Keplerian → Kozai → TLE
|
|
*
|
|
* Memory: uses palloc/pfree when compiled with PostgreSQL,
|
|
* malloc/free otherwise (controlled by OD_USE_PALLOC).
|
|
*/
|
|
|
|
#include "od_solver.h"
|
|
#include "od_math.h"
|
|
#include "od_iod.h"
|
|
#include "norad.h"
|
|
#include "norad_in.h"
|
|
|
|
#include <math.h>
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
|
|
/* When compiled as part of the PostgreSQL extension, use palloc.
|
|
* The Makefile sets PG_CPPFLAGS which pulls in postgres.h via types.h,
|
|
* but od_solver.c doesn't include types.h directly. We detect via
|
|
* the presence of PGXS-defined macros. */
|
|
#ifdef PG_MODULE_MAGIC
|
|
#include "postgres.h"
|
|
#define od_alloc(sz) palloc(sz)
|
|
#define od_free(p) pfree(p)
|
|
#else
|
|
#include <stdlib.h>
|
|
#define od_alloc(sz) malloc(sz)
|
|
#define od_free(p) free(p)
|
|
#endif
|
|
|
|
/* ── LAPACK interface ──────────────────────────────────── */
|
|
|
|
/*
|
|
* dgelss_: solve min ||Ax - b|| via SVD.
|
|
* A is M x N, b is M x 1, solution returned in b.
|
|
* Column-major storage (Fortran convention).
|
|
*/
|
|
extern void dgelss_(int *m, int *n, int *nrhs,
|
|
double *a, int *lda,
|
|
double *b, int *ldb,
|
|
double *s, double *rcond, int *rank,
|
|
double *work, int *lwork, int *info);
|
|
|
|
/*
|
|
* dpotrf_: Cholesky factorization of a symmetric positive-definite matrix.
|
|
* dpotri_: Invert using the Cholesky factor from dpotrf_.
|
|
* Used together to compute (H^T H)^{-1} for covariance output.
|
|
*/
|
|
extern void dpotrf_(char *uplo, int *n, double *a, int *lda, int *info);
|
|
extern void dpotri_(char *uplo, int *n, double *a, int *lda, int *info);
|
|
|
|
|
|
/* ── Internal propagation wrapper ─────────────────────── */
|
|
|
|
/*
|
|
* Propagate a tle_t to tsince minutes, returning pos[3] (km)
|
|
* and vel[3] (km/min). Reuses a pre-allocated params buffer.
|
|
* Returns SGP4/SDP4 error code (0 = success).
|
|
*/
|
|
static int
|
|
propagate_with_params(const tle_t *sat, double tsince,
|
|
double *params, int is_deep,
|
|
double pos[3], double vel[3])
|
|
{
|
|
if (is_deep)
|
|
return SDP4(tsince, sat, params, pos, vel);
|
|
else
|
|
return SGP4(tsince, sat, params, pos, vel);
|
|
}
|
|
|
|
static void
|
|
init_params(const tle_t *sat, double *params, int is_deep)
|
|
{
|
|
if (is_deep)
|
|
SDP4_init(params, sat);
|
|
else
|
|
SGP4_init(params, sat);
|
|
}
|
|
|
|
|
|
/* ── Equinoctial ↔ TLE conversion ─────────────────────── */
|
|
|
|
/*
|
|
* Extract equinoctial elements from a tle_t.
|
|
*
|
|
* TLE stores Kozai mean motion. We first convert to Brouwer mean
|
|
* motion (which is what the equinoctial elements represent in the
|
|
* fitted state), then compute the equinoctial set.
|
|
*/
|
|
static void
|
|
tle_to_equinoctial(const tle_t *tle, od_equinoctial_t *eq)
|
|
{
|
|
od_keplerian_t kep;
|
|
|
|
/* TLE xno is Kozai mean motion → convert to Brouwer */
|
|
kep.n = od_kozai_to_brouwer_n(tle->xno, tle->eo, tle->xincl);
|
|
kep.ecc = tle->eo;
|
|
kep.inc = tle->xincl;
|
|
kep.raan = tle->xnodeo;
|
|
kep.argp = tle->omegao;
|
|
kep.M = tle->xmo;
|
|
kep.bstar = tle->bstar;
|
|
|
|
od_keplerian_to_equinoctial(&kep, eq);
|
|
}
|
|
|
|
/*
|
|
* Build a tle_t from equinoctial elements and a template TLE
|
|
* (for metadata: NORAD ID, classification, intl desig, etc).
|
|
*
|
|
* Converts equinoctial → Keplerian → Kozai mean motion.
|
|
*/
|
|
static void
|
|
equinoctial_to_tle(const od_equinoctial_t *eq, const tle_t *tmpl,
|
|
tle_t *out)
|
|
{
|
|
od_keplerian_t kep;
|
|
|
|
od_equinoctial_to_keplerian(eq, &kep);
|
|
|
|
memset(out, 0, sizeof(tle_t));
|
|
|
|
/* Orbital elements */
|
|
out->epoch = tmpl->epoch;
|
|
out->xincl = kep.inc;
|
|
out->xnodeo = od_normalize_angle(kep.raan);
|
|
out->eo = kep.ecc;
|
|
out->omegao = od_normalize_angle(kep.argp);
|
|
out->xmo = od_normalize_angle(kep.M);
|
|
|
|
/* Brouwer n → Kozai n for TLE storage */
|
|
out->xno = od_brouwer_to_kozai_n(kep.n, kep.ecc, kep.inc);
|
|
|
|
/* Drag terms */
|
|
out->bstar = eq->bstar;
|
|
out->xndt2o = 0.0; /* not fitted */
|
|
out->xndd6o = 0.0; /* not fitted */
|
|
|
|
/* Copy metadata from template */
|
|
out->norad_number = tmpl->norad_number;
|
|
out->bulletin_number = tmpl->bulletin_number;
|
|
out->revolution_number = tmpl->revolution_number;
|
|
out->classification = tmpl->classification;
|
|
out->ephemeris_type = tmpl->ephemeris_type;
|
|
memcpy(out->intl_desig, tmpl->intl_desig, 9);
|
|
}
|
|
|
|
|
|
/* ── Compute residuals ─────────────────────────────────── */
|
|
|
|
/*
|
|
* Compute residual vector for ECI observations.
|
|
* residuals[i*6 .. i*6+5] = observed[i] - propagated[i]
|
|
* Returns RMS position error in km. Returns -1.0 on propagation failure.
|
|
*/
|
|
static double
|
|
compute_residuals_eci(const tle_t *tle, const od_observation_t *obs,
|
|
int n_obs, double *residuals)
|
|
{
|
|
double *params;
|
|
int is_deep;
|
|
double sum_sq = 0.0;
|
|
int i;
|
|
|
|
is_deep = select_ephemeris(tle);
|
|
if (is_deep < 0)
|
|
return -1.0;
|
|
|
|
params = (double *)od_alloc(sizeof(double) * N_SAT_PARAMS);
|
|
init_params(tle, params, is_deep);
|
|
|
|
for (i = 0; i < n_obs; i++)
|
|
{
|
|
double tsince = (obs[i].jd - tle->epoch) * 1440.0;
|
|
double pos[3], vel[3];
|
|
int err;
|
|
double dx, dy, dz;
|
|
|
|
err = propagate_with_params(tle, tsince, params, is_deep, pos, vel);
|
|
if (err != 0 && err != SXPX_WARN_ORBIT_WITHIN_EARTH &&
|
|
err != SXPX_WARN_PERIGEE_WITHIN_EARTH)
|
|
{
|
|
od_free(params);
|
|
return -1.0;
|
|
}
|
|
|
|
/* SGP4 outputs vel in km/min; convert to km/s for comparison */
|
|
vel[0] /= 60.0;
|
|
vel[1] /= 60.0;
|
|
vel[2] /= 60.0;
|
|
|
|
/* Residual = observed - computed */
|
|
dx = obs[i].data[0] - pos[0];
|
|
dy = obs[i].data[1] - pos[1];
|
|
dz = obs[i].data[2] - pos[2];
|
|
|
|
residuals[i * 6 + 0] = dx;
|
|
residuals[i * 6 + 1] = dy;
|
|
residuals[i * 6 + 2] = dz;
|
|
residuals[i * 6 + 3] = obs[i].data[3] - vel[0];
|
|
residuals[i * 6 + 4] = obs[i].data[4] - vel[1];
|
|
residuals[i * 6 + 5] = obs[i].data[5] - vel[2];
|
|
|
|
sum_sq += dx * dx + dy * dy + dz * dz;
|
|
}
|
|
|
|
od_free(params);
|
|
return sqrt(sum_sq / n_obs);
|
|
}
|
|
|
|
/*
|
|
* Compute residual vector for topocentric observations.
|
|
* residuals[i*3 .. i*3+2] = observed[i] - propagated[i]
|
|
* Components: (az_diff, el_diff, range_diff) in (rad, rad, km).
|
|
* Returns RMS range error in km. Returns -1.0 on propagation failure.
|
|
*/
|
|
static double
|
|
compute_residuals_topo(const tle_t *tle, const od_observation_t *obs,
|
|
int n_obs, const od_observer_t *observers,
|
|
double *residuals)
|
|
{
|
|
double *params;
|
|
int is_deep;
|
|
double sum_sq = 0.0;
|
|
int i;
|
|
|
|
is_deep = select_ephemeris(tle);
|
|
if (is_deep < 0)
|
|
return -1.0;
|
|
|
|
params = (double *)od_alloc(sizeof(double) * N_SAT_PARAMS);
|
|
init_params(tle, params, is_deep);
|
|
|
|
for (i = 0; i < n_obs; i++)
|
|
{
|
|
const od_observer_t *observer = &observers[obs[i].observer_idx];
|
|
double tsince = (obs[i].jd - tle->epoch) * 1440.0;
|
|
double pos[3], vel[3];
|
|
int err;
|
|
double gmst;
|
|
double pos_ecef[3], vel_ecef[3];
|
|
double cos_g, sin_g;
|
|
double dx, dy, dz;
|
|
double sin_lat, cos_lat, sin_lon, cos_lon;
|
|
double south, east, up;
|
|
double range_km, az, el;
|
|
double az_diff, el_diff, range_diff;
|
|
|
|
err = propagate_with_params(tle, tsince, params, is_deep, pos, vel);
|
|
if (err != 0 && err != SXPX_WARN_ORBIT_WITHIN_EARTH &&
|
|
err != SXPX_WARN_PERIGEE_WITHIN_EARTH)
|
|
{
|
|
od_free(params);
|
|
return -1.0;
|
|
}
|
|
|
|
/* TEME → ECEF (replicate coord_funcs.c pipeline) */
|
|
gmst = od_gmst_from_jd(obs[i].jd);
|
|
cos_g = cos(gmst);
|
|
sin_g = sin(gmst);
|
|
|
|
pos_ecef[0] = cos_g * pos[0] + sin_g * pos[1];
|
|
pos_ecef[1] = -sin_g * pos[0] + cos_g * pos[1];
|
|
pos_ecef[2] = pos[2];
|
|
|
|
/* vel in km/min → km/s for ECEF velocity */
|
|
vel[0] /= 60.0;
|
|
vel[1] /= 60.0;
|
|
vel[2] /= 60.0;
|
|
|
|
vel_ecef[0] = cos_g * vel[0] + sin_g * vel[1]
|
|
+ 7.2921158553e-5 * pos_ecef[1];
|
|
vel_ecef[1] = -sin_g * vel[0] + cos_g * vel[1]
|
|
- 7.2921158553e-5 * pos_ecef[0];
|
|
vel_ecef[2] = vel[2];
|
|
|
|
/* ECEF → topocentric */
|
|
dx = pos_ecef[0] - observer->ecef[0];
|
|
dy = pos_ecef[1] - observer->ecef[1];
|
|
dz = pos_ecef[2] - observer->ecef[2];
|
|
|
|
sin_lat = sin(observer->lat);
|
|
cos_lat = cos(observer->lat);
|
|
sin_lon = sin(observer->lon);
|
|
cos_lon = cos(observer->lon);
|
|
|
|
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;
|
|
|
|
range_km = sqrt(dx*dx + dy*dy + dz*dz);
|
|
el = asin(up / range_km);
|
|
az = atan2(east, -south);
|
|
if (az < 0.0) az += 2.0 * M_PI;
|
|
|
|
/* Residuals in topo space */
|
|
az_diff = obs[i].data[0] - az;
|
|
/* Handle azimuth wrap-around */
|
|
if (az_diff > M_PI) az_diff -= 2.0 * M_PI;
|
|
if (az_diff < -M_PI) az_diff += 2.0 * M_PI;
|
|
|
|
el_diff = obs[i].data[1] - el;
|
|
range_diff = obs[i].data[2] - range_km;
|
|
|
|
residuals[i * 3 + 0] = az_diff * range_km * cos(el); /* scale to km */
|
|
residuals[i * 3 + 1] = el_diff * range_km; /* scale to km */
|
|
residuals[i * 3 + 2] = range_diff;
|
|
|
|
sum_sq += range_diff * range_diff;
|
|
|
|
(void)vel_ecef; /* reserved for range rate fitting */
|
|
}
|
|
|
|
od_free(params);
|
|
return sqrt(sum_sq / n_obs);
|
|
}
|
|
|
|
|
|
/* ── Step limiting ─────────────────────────────────────── */
|
|
|
|
/*
|
|
* Apply Vallado 2008 Section V.B step limits to the correction vector.
|
|
*
|
|
* Each element has a maximum allowable step. If the correction exceeds
|
|
* the limit, it's scaled down. B* gets special treatment because it
|
|
* operates on a different scale.
|
|
*/
|
|
static void
|
|
apply_step_limits(double *dx, int nstate, const od_equinoctial_t *eq,
|
|
double adaptive_factor)
|
|
{
|
|
/* Step limits for equinoctial elements (Vallado 2008 Table 2) */
|
|
static const double limits_6[6] = {
|
|
0.01, /* n: 1% of typical LEO mean motion */
|
|
0.1, /* af: eccentricity-related */
|
|
0.1, /* ag: eccentricity-related */
|
|
0.1, /* chi: inclination-related */
|
|
0.1, /* psi: inclination-related */
|
|
0.5 /* L0: mean longitude */
|
|
};
|
|
|
|
int i;
|
|
double ratio, max_ratio = 0.0;
|
|
|
|
/* Find the maximum ratio of correction to limit, scaled by
|
|
* adaptive_factor (trust-region-like damping when diverging) */
|
|
for (i = 0; i < 6; i++)
|
|
{
|
|
double scale;
|
|
if (i == 0)
|
|
scale = fabs(eq->n) > 1e-10 ? fabs(eq->n) : 1e-3;
|
|
else
|
|
scale = 1.0;
|
|
|
|
ratio = fabs(dx[i]) / (limits_6[i] * scale * adaptive_factor);
|
|
if (ratio > max_ratio)
|
|
max_ratio = ratio;
|
|
}
|
|
|
|
/* Scale all corrections proportionally if any exceeds its limit */
|
|
if (max_ratio > 1.0)
|
|
{
|
|
double scale_factor = 1.0 / max_ratio;
|
|
for (i = 0; i < 6; i++)
|
|
dx[i] *= scale_factor;
|
|
}
|
|
|
|
/* B* special treatment: 40% limit when ratio > 1.0 */
|
|
if (nstate == OD_NSTATE_7)
|
|
{
|
|
double bstar_limit = fabs(eq->bstar) > 1e-10
|
|
? fabs(eq->bstar) * 0.4 * adaptive_factor
|
|
: 1e-5 * adaptive_factor;
|
|
if (fabs(dx[6]) > bstar_limit)
|
|
dx[6] = (dx[6] > 0.0 ? bstar_limit : -bstar_limit);
|
|
}
|
|
}
|
|
|
|
|
|
/* ── Physical bounds enforcement ───────────────────────── */
|
|
|
|
/*
|
|
* Clamp equinoctial elements to physically valid ranges.
|
|
* Prevents the solver from wandering into nonsense.
|
|
*/
|
|
static void
|
|
enforce_bounds(od_equinoctial_t *eq)
|
|
{
|
|
double ecc = sqrt(eq->af * eq->af + eq->ag * eq->ag);
|
|
|
|
/* Eccentricity must be in [0, 0.999] */
|
|
if (ecc > 0.999)
|
|
{
|
|
double scale = 0.999 / ecc;
|
|
eq->af *= scale;
|
|
eq->ag *= scale;
|
|
}
|
|
|
|
/* Mean motion must be positive */
|
|
if (eq->n < 1e-8)
|
|
eq->n = 1e-8;
|
|
|
|
/* Semi-major axis must be >= 1 Earth radius (WGS-72) */
|
|
{
|
|
double a_er = pow(xke / eq->n, two_thirds);
|
|
if (a_er < 1.0)
|
|
{
|
|
eq->n = xke; /* set to a = 1 ER */
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/* ── Initial guess from observations ───────────────────── */
|
|
|
|
/*
|
|
* Helper: populate a TLE from Keplerian elements and an epoch.
|
|
*/
|
|
static void
|
|
kep_to_seed_tle(const od_keplerian_t *kep, double epoch_jd, tle_t *guess)
|
|
{
|
|
memset(guess, 0, sizeof(tle_t));
|
|
guess->epoch = epoch_jd;
|
|
guess->xincl = kep->inc;
|
|
guess->xnodeo = od_normalize_angle(kep->raan);
|
|
guess->eo = kep->ecc;
|
|
guess->omegao = od_normalize_angle(kep->argp);
|
|
guess->xmo = od_normalize_angle(kep->M);
|
|
guess->xno = od_brouwer_to_kozai_n(kep->n, kep->ecc, kep->inc);
|
|
guess->bstar = 0.0;
|
|
guess->norad_number = 99999;
|
|
guess->classification = 'U';
|
|
guess->ephemeris_type = '0';
|
|
}
|
|
|
|
/*
|
|
* Compute initial orbit from ECI observations.
|
|
*
|
|
* ECI observations include full velocity, so single-obs r,v→elements
|
|
* is exact for two-body and typically superior to Gibbs (which only
|
|
* uses positions). Gibbs is only attempted as fallback when the
|
|
* single-obs conversion fails (degenerate orbit).
|
|
*/
|
|
static void
|
|
initial_guess_from_eci(const od_observation_t *obs, int n_obs,
|
|
tle_t *guess)
|
|
{
|
|
od_keplerian_t kep;
|
|
|
|
/* Primary: use first observation's position/velocity (exact for
|
|
* two-body, excellent for short-arc LEO) */
|
|
if (od_eci_to_keplerian(obs[0].data, obs[0].data + 3, &kep) == 0)
|
|
{
|
|
kep_to_seed_tle(&kep, obs[0].jd, guess);
|
|
return;
|
|
}
|
|
|
|
/* Fallback: Gibbs from 3 well-spaced positions */
|
|
if (n_obs >= 3)
|
|
{
|
|
int i0 = 0, i1 = n_obs / 2, i2 = n_obs - 1;
|
|
od_iod_result_t iod;
|
|
|
|
if (od_gibbs(obs[i0].data, obs[i1].data, obs[i2].data,
|
|
obs[i0].jd, obs[i1].jd, obs[i2].jd,
|
|
&iod) == 0)
|
|
{
|
|
kep_to_seed_tle(&iod.kep, iod.epoch_jd, guess);
|
|
return;
|
|
}
|
|
}
|
|
|
|
/* Last resort: zero elements (solver will struggle) */
|
|
memset(guess, 0, sizeof(tle_t));
|
|
guess->epoch = obs[0].jd;
|
|
guess->norad_number = 99999;
|
|
guess->classification = 'U';
|
|
guess->ephemeris_type = '0';
|
|
}
|
|
|
|
/*
|
|
* Compute initial orbit from topocentric observations.
|
|
*
|
|
* Converts 3 well-spaced topo observations to TEME positions
|
|
* via topo→ECEF→TEME, then uses Gibbs to recover the orbit.
|
|
* Returns 0 on success, -1 if Gibbs fails.
|
|
*/
|
|
static int
|
|
initial_guess_from_topo(const od_observation_t *obs, int n_obs,
|
|
const od_observer_t *observers,
|
|
tle_t *guess)
|
|
{
|
|
int idx[3];
|
|
double pos_teme[3][3];
|
|
double jd[3];
|
|
int k;
|
|
od_iod_result_t iod;
|
|
|
|
if (n_obs < 3)
|
|
return -1;
|
|
|
|
idx[0] = 0;
|
|
idx[1] = n_obs / 2;
|
|
idx[2] = n_obs - 1;
|
|
|
|
for (k = 0; k < 3; k++)
|
|
{
|
|
const od_observation_t *o = &obs[idx[k]];
|
|
const od_observer_t *observer = &observers[o->observer_idx];
|
|
double sat_ecef[3];
|
|
double gmst;
|
|
double vel_zero[3] = {0, 0, 0};
|
|
|
|
/* Topo → ECEF */
|
|
od_topocentric_to_ecef(o->data[0], o->data[1], o->data[2],
|
|
observer->ecef, observer->lat, observer->lon,
|
|
sat_ecef);
|
|
|
|
/* ECEF → TEME */
|
|
gmst = od_gmst_from_jd(o->jd);
|
|
od_ecef_to_teme(sat_ecef, vel_zero, gmst,
|
|
pos_teme[k], vel_zero);
|
|
|
|
jd[k] = o->jd;
|
|
}
|
|
|
|
if (od_gibbs(pos_teme[0], pos_teme[1], pos_teme[2],
|
|
jd[0], jd[1], jd[2], &iod) != 0)
|
|
return -1;
|
|
|
|
kep_to_seed_tle(&iod.kep, iod.epoch_jd, guess);
|
|
return 0;
|
|
}
|
|
|
|
|
|
/* ── Main solver ───────────────────────────────────────── */
|
|
|
|
int
|
|
od_fit_tle(const od_observation_t *obs, int n_obs,
|
|
const tle_t *seed, const od_config_t *config,
|
|
od_result_t *result)
|
|
{
|
|
int nstate; /* 6 or 7 */
|
|
int ncomp; /* components per observation: 6 (ECI) or 3 (topo) */
|
|
int nrows; /* total residual components: ncomp * n_obs */
|
|
od_equinoctial_t eq;
|
|
tle_t current_tle;
|
|
tle_t seed_tle;
|
|
double *residuals = NULL;
|
|
double *jacobian = NULL;
|
|
double *dx = NULL;
|
|
double *sv = NULL;
|
|
double *work = NULL;
|
|
double rms_prev, rms_cur;
|
|
double adaptive_factor = 1.0; /* trust-region damping */
|
|
double condition_number = 0.0; /* s_max / s_min from SVD */
|
|
int iter;
|
|
int max_iter;
|
|
int converged = 0;
|
|
int j_cov, k_cov; /* covariance loop indices */
|
|
|
|
/* Validate inputs */
|
|
nstate = config->fit_bstar ? OD_NSTATE_7 : OD_NSTATE_6;
|
|
ncomp = (config->obs_type == OD_OBS_ECI) ? 6 : 3;
|
|
|
|
if (n_obs < nstate)
|
|
{
|
|
snprintf(result->status, sizeof(result->status),
|
|
"insufficient observations: %d < %d", n_obs, nstate);
|
|
return -1;
|
|
}
|
|
|
|
nrows = ncomp * n_obs;
|
|
max_iter = config->max_iter > 0 ? config->max_iter : OD_MAX_ITER;
|
|
if (max_iter > OD_MAX_ITER)
|
|
max_iter = OD_MAX_ITER;
|
|
|
|
/* Initialize seed TLE */
|
|
if (seed)
|
|
{
|
|
memcpy(&seed_tle, seed, sizeof(tle_t));
|
|
}
|
|
else
|
|
{
|
|
if (config->obs_type == OD_OBS_ECI)
|
|
initial_guess_from_eci(obs, n_obs, &seed_tle);
|
|
else
|
|
{
|
|
int rc_iod = initial_guess_from_topo(obs, n_obs,
|
|
config->observers,
|
|
&seed_tle);
|
|
if (rc_iod != 0)
|
|
{
|
|
snprintf(result->status, sizeof(result->status),
|
|
"IOD bootstrap failed (Gibbs): need seed TLE");
|
|
return -1;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* Extract equinoctial elements from seed */
|
|
tle_to_equinoctial(&seed_tle, &eq);
|
|
|
|
/* Allocate working arrays */
|
|
residuals = (double *)od_alloc(sizeof(double) * nrows);
|
|
jacobian = (double *)od_alloc(sizeof(double) * nrows * nstate);
|
|
dx = (double *)od_alloc(sizeof(double) * (nrows > nstate ? nrows : nstate));
|
|
sv = (double *)od_alloc(sizeof(double) * nstate);
|
|
|
|
/* LAPACK workspace query */
|
|
{
|
|
int m = nrows, n = nstate, nrhs = 1, lda = nrows, ldb = nrows;
|
|
int rank, info, lwork = -1;
|
|
double rcond = -1.0;
|
|
double work_query;
|
|
|
|
dgelss_(&m, &n, &nrhs, jacobian, &lda, dx, &ldb,
|
|
sv, &rcond, &rank, &work_query, &lwork, &info);
|
|
lwork = (int)work_query + 1;
|
|
work = (double *)od_alloc(sizeof(double) * lwork);
|
|
}
|
|
|
|
/* Build initial TLE from equinoctial state */
|
|
equinoctial_to_tle(&eq, &seed_tle, ¤t_tle);
|
|
|
|
/* Compute initial RMS */
|
|
if (config->obs_type == OD_OBS_ECI)
|
|
rms_cur = compute_residuals_eci(¤t_tle, obs, n_obs, residuals);
|
|
else
|
|
rms_cur = compute_residuals_topo(¤t_tle, obs, n_obs,
|
|
config->observers, residuals);
|
|
|
|
if (rms_cur < 0.0)
|
|
{
|
|
snprintf(result->status, sizeof(result->status),
|
|
"initial propagation failed");
|
|
od_free(residuals);
|
|
od_free(jacobian);
|
|
od_free(dx);
|
|
od_free(sv);
|
|
od_free(work);
|
|
return -1;
|
|
}
|
|
|
|
result->rms_initial = rms_cur;
|
|
rms_prev = rms_cur;
|
|
|
|
/* Already converged (perfect seed)? Skip DC loop but still
|
|
* compute covariance — users need uncertainty estimates even
|
|
* when the initial guess is exact. */
|
|
if (rms_cur < OD_RMS_ABS_TOL)
|
|
{
|
|
converged = 1;
|
|
iter = 0;
|
|
goto compute_covariance;
|
|
}
|
|
|
|
/* ── DC iteration loop ─────────────────────────────── */
|
|
|
|
for (iter = 0; iter < max_iter; iter++)
|
|
{
|
|
int j, k;
|
|
int m, n, nrhs, lda, ldb, rank, info, lwork_val;
|
|
double rcond;
|
|
|
|
/* Build Jacobian by finite differencing */
|
|
for (j = 0; j < nstate; j++)
|
|
{
|
|
od_equinoctial_t eq_pert;
|
|
tle_t tle_pert;
|
|
double *resid_pert;
|
|
double h;
|
|
double elem_val;
|
|
|
|
memcpy(&eq_pert, &eq, sizeof(od_equinoctial_t));
|
|
|
|
/* Element value for scaling the perturbation */
|
|
switch (j)
|
|
{
|
|
case 0: elem_val = eq.n; break;
|
|
case 1: elem_val = eq.af; break;
|
|
case 2: elem_val = eq.ag; break;
|
|
case 3: elem_val = eq.chi; break;
|
|
case 4: elem_val = eq.psi; break;
|
|
case 5: elem_val = eq.L0; break;
|
|
case 6: elem_val = eq.bstar; break;
|
|
default: elem_val = 0.0; break;
|
|
}
|
|
|
|
/* Perturbation step: max of relative and absolute */
|
|
h = fabs(elem_val) * 1e-6;
|
|
if (h < 1e-10)
|
|
h = 1e-10;
|
|
|
|
/* Apply perturbation */
|
|
switch (j)
|
|
{
|
|
case 0: eq_pert.n += h; break;
|
|
case 1: eq_pert.af += h; break;
|
|
case 2: eq_pert.ag += h; break;
|
|
case 3: eq_pert.chi += h; break;
|
|
case 4: eq_pert.psi += h; break;
|
|
case 5: eq_pert.L0 += h; break;
|
|
case 6: eq_pert.bstar += h; break;
|
|
}
|
|
|
|
equinoctial_to_tle(&eq_pert, &seed_tle, &tle_pert);
|
|
|
|
resid_pert = (double *)od_alloc(sizeof(double) * nrows);
|
|
|
|
if (config->obs_type == OD_OBS_ECI)
|
|
compute_residuals_eci(&tle_pert, obs, n_obs, resid_pert);
|
|
else
|
|
compute_residuals_topo(&tle_pert, obs, n_obs,
|
|
config->observers, resid_pert);
|
|
|
|
/* Jacobian column j (column-major for LAPACK)
|
|
* H = dG/dx where G is the computed observation function.
|
|
* resid = obs - G(x), resid_pert = obs - G(x + h*e_j), so:
|
|
* (resid - resid_pert) / h = (G(x+h) - G(x)) / h = dG/dx_j */
|
|
for (k = 0; k < nrows; k++)
|
|
jacobian[j * nrows + k] = (residuals[k] - resid_pert[k]) / h;
|
|
|
|
od_free(resid_pert);
|
|
}
|
|
|
|
/* Copy residuals into dx (LAPACK overwrites with solution).
|
|
* Jacobian is H = dG/dx, and residual = obs - G(x), so
|
|
* LAPACK solves H*delta = residual (standard DC). */
|
|
memcpy(dx, residuals, sizeof(double) * nrows);
|
|
|
|
/* Solve via SVD */
|
|
m = nrows;
|
|
n = nstate;
|
|
nrhs = 1;
|
|
lda = nrows;
|
|
ldb = nrows;
|
|
rcond = -1.0; /* use machine epsilon */
|
|
{
|
|
/* Re-query workspace since dimensions haven't changed */
|
|
int lwork_query = -1;
|
|
double wq;
|
|
dgelss_(&m, &n, &nrhs, jacobian, &lda, dx, &ldb,
|
|
sv, &rcond, &rank, &wq, &lwork_query, &info);
|
|
lwork_val = (int)wq + 1;
|
|
|
|
/* Reallocate work if needed */
|
|
od_free(work);
|
|
work = (double *)od_alloc(sizeof(double) * lwork_val);
|
|
}
|
|
|
|
dgelss_(&m, &n, &nrhs, jacobian, &lda, dx, &ldb,
|
|
sv, &rcond, &rank, work, &lwork_val, &info);
|
|
|
|
if (info != 0)
|
|
{
|
|
snprintf(result->status, sizeof(result->status),
|
|
"LAPACK dgelss_ failed: info=%d", info);
|
|
break;
|
|
}
|
|
|
|
/* Store condition number from SVD singular values */
|
|
if (sv[nstate - 1] > 1e-30)
|
|
condition_number = sv[0] / sv[nstate - 1];
|
|
|
|
/* Apply step limiting (scaled by adaptive factor) */
|
|
apply_step_limits(dx, nstate, &eq, adaptive_factor);
|
|
|
|
/* Update equinoctial elements */
|
|
eq.n += dx[0];
|
|
eq.af += dx[1];
|
|
eq.ag += dx[2];
|
|
eq.chi += dx[3];
|
|
eq.psi += dx[4];
|
|
eq.L0 += dx[5];
|
|
if (nstate == OD_NSTATE_7)
|
|
eq.bstar += dx[6];
|
|
|
|
eq.L0 = od_normalize_angle(eq.L0);
|
|
|
|
/* Enforce physical bounds */
|
|
enforce_bounds(&eq);
|
|
|
|
/* Rebuild TLE and compute new residuals */
|
|
equinoctial_to_tle(&eq, &seed_tle, ¤t_tle);
|
|
|
|
if (config->obs_type == OD_OBS_ECI)
|
|
rms_cur = compute_residuals_eci(¤t_tle, obs, n_obs, residuals);
|
|
else
|
|
rms_cur = compute_residuals_topo(¤t_tle, obs, n_obs,
|
|
config->observers, residuals);
|
|
|
|
if (rms_cur < 0.0)
|
|
{
|
|
snprintf(result->status, sizeof(result->status),
|
|
"propagation failed at iteration %d", iter + 1);
|
|
break;
|
|
}
|
|
|
|
/* Adaptive step control: adjust trust-region size based on
|
|
* whether the solver is converging or diverging. Halve the
|
|
* step on divergence (prevents oscillation with poor initial
|
|
* guesses), relax toward full step when converging well. */
|
|
if (rms_cur > rms_prev * 1.01)
|
|
{
|
|
adaptive_factor *= 0.5;
|
|
if (adaptive_factor < 0.05)
|
|
adaptive_factor = 0.05;
|
|
}
|
|
else if (rms_cur < rms_prev * 0.9)
|
|
{
|
|
adaptive_factor = fmin(adaptive_factor * 1.3, 1.0);
|
|
}
|
|
|
|
/* Check convergence: either absolute RMS is small enough,
|
|
* or relative change has plateaued (solver found its best fit,
|
|
* common for SDP4 deep-space orbits where the numerical floor
|
|
* is ~meters). */
|
|
{
|
|
double rms_change = fabs(rms_cur - rms_prev);
|
|
double rms_rel = (rms_prev > 1e-15)
|
|
? rms_change / rms_prev
|
|
: rms_change;
|
|
|
|
if (rms_rel < OD_RMS_REL_TOL || rms_cur < OD_RMS_ABS_TOL)
|
|
{
|
|
converged = 1;
|
|
iter++; /* count this iteration */
|
|
break;
|
|
}
|
|
}
|
|
|
|
rms_prev = rms_cur;
|
|
}
|
|
|
|
compute_covariance:
|
|
/* ── Covariance computation ─────────────────────────── */
|
|
|
|
/*
|
|
* Compute formal covariance matrix (H^T H)^{-1} at the converged
|
|
* solution. This requires rebuilding the Jacobian at the final state
|
|
* (the DC loop overwrites it each iteration via LAPACK).
|
|
*
|
|
* The covariance gives parameter uncertainty estimates — diagonal
|
|
* elements are variances, off-diagonals are correlations. Essential
|
|
* for conjunction screening probability computations.
|
|
*/
|
|
result->cov_size = 0;
|
|
result->condition_number = condition_number;
|
|
|
|
if (converged || iter >= max_iter)
|
|
{
|
|
/* Rebuild Jacobian at final state (same finite-diff as DC loop) */
|
|
for (j_cov = 0; j_cov < nstate; j_cov++)
|
|
{
|
|
od_equinoctial_t eq_pert;
|
|
tle_t tle_pert;
|
|
double *resid_pert;
|
|
double h, elem_val;
|
|
|
|
memcpy(&eq_pert, &eq, sizeof(od_equinoctial_t));
|
|
|
|
switch (j_cov)
|
|
{
|
|
case 0: elem_val = eq.n; break;
|
|
case 1: elem_val = eq.af; break;
|
|
case 2: elem_val = eq.ag; break;
|
|
case 3: elem_val = eq.chi; break;
|
|
case 4: elem_val = eq.psi; break;
|
|
case 5: elem_val = eq.L0; break;
|
|
case 6: elem_val = eq.bstar; break;
|
|
default: elem_val = 0.0; break;
|
|
}
|
|
|
|
h = fabs(elem_val) * 1e-6;
|
|
if (h < 1e-10)
|
|
h = 1e-10;
|
|
|
|
switch (j_cov)
|
|
{
|
|
case 0: eq_pert.n += h; break;
|
|
case 1: eq_pert.af += h; break;
|
|
case 2: eq_pert.ag += h; break;
|
|
case 3: eq_pert.chi += h; break;
|
|
case 4: eq_pert.psi += h; break;
|
|
case 5: eq_pert.L0 += h; break;
|
|
case 6: eq_pert.bstar += h; break;
|
|
}
|
|
|
|
equinoctial_to_tle(&eq_pert, &seed_tle, &tle_pert);
|
|
|
|
resid_pert = (double *)od_alloc(sizeof(double) * nrows);
|
|
|
|
if (config->obs_type == OD_OBS_ECI)
|
|
compute_residuals_eci(&tle_pert, obs, n_obs, resid_pert);
|
|
else
|
|
compute_residuals_topo(&tle_pert, obs, n_obs,
|
|
config->observers, resid_pert);
|
|
|
|
for (k_cov = 0; k_cov < nrows; k_cov++)
|
|
jacobian[j_cov * nrows + k_cov] =
|
|
(residuals[k_cov] - resid_pert[k_cov]) / h;
|
|
|
|
od_free(resid_pert);
|
|
}
|
|
|
|
/* Compute H^T H before SVD destroys the Jacobian */
|
|
{
|
|
double *hth;
|
|
int ci, cj, ri, info_cov;
|
|
char uplo = 'U';
|
|
|
|
hth = (double *)od_alloc(sizeof(double) * nstate * nstate);
|
|
memset(hth, 0, sizeof(double) * nstate * nstate);
|
|
|
|
for (ci = 0; ci < nstate; ci++)
|
|
{
|
|
for (cj = ci; cj < nstate; cj++)
|
|
{
|
|
double dot = 0.0;
|
|
for (ri = 0; ri < nrows; ri++)
|
|
dot += jacobian[ci * nrows + ri] *
|
|
jacobian[cj * nrows + ri];
|
|
hth[ci + cj * nstate] = dot;
|
|
hth[cj + ci * nstate] = dot;
|
|
}
|
|
}
|
|
|
|
/* SVD of Jacobian for condition number (destroys jacobian) */
|
|
{
|
|
int m = nrows, n = nstate, nrhs = 1;
|
|
int lda = nrows, ldb = nrows, rank, info_svd;
|
|
double rcond = -1.0;
|
|
|
|
memcpy(dx, residuals, sizeof(double) * nrows);
|
|
|
|
od_free(work);
|
|
{
|
|
int lw = -1;
|
|
double wq;
|
|
dgelss_(&m, &n, &nrhs, jacobian, &lda, dx, &ldb,
|
|
sv, &rcond, &rank, &wq, &lw, &info_svd);
|
|
lw = (int)wq + 1;
|
|
work = (double *)od_alloc(sizeof(double) * lw);
|
|
dgelss_(&m, &n, &nrhs, jacobian, &lda, dx, &ldb,
|
|
sv, &rcond, &rank, work, &lw, &info_svd);
|
|
}
|
|
|
|
if (info_svd == 0 && sv[nstate - 1] > 1e-30)
|
|
condition_number = sv[0] / sv[nstate - 1];
|
|
}
|
|
|
|
result->condition_number = condition_number;
|
|
|
|
/* Cholesky factorize H^T H, then invert for covariance */
|
|
dpotrf_(&uplo, &nstate, hth, &nstate, &info_cov);
|
|
|
|
if (info_cov == 0)
|
|
{
|
|
dpotri_(&uplo, &nstate, hth, &nstate, &info_cov);
|
|
|
|
if (info_cov == 0)
|
|
{
|
|
int idx = 0;
|
|
for (ci = 0; ci < nstate; ci++)
|
|
for (cj = ci; cj < nstate; cj++)
|
|
result->covariance[idx++] = hth[ci + cj * nstate];
|
|
result->cov_size = idx; /* 21 or 28 */
|
|
}
|
|
}
|
|
|
|
od_free(hth);
|
|
}
|
|
}
|
|
|
|
/* ── Build result ──────────────────────────────────── */
|
|
|
|
memcpy(&result->fitted_tle, ¤t_tle, sizeof(tle_t));
|
|
result->iterations = iter;
|
|
result->rms_final = rms_cur;
|
|
result->converged = converged;
|
|
|
|
if (converged)
|
|
snprintf(result->status, sizeof(result->status), "converged");
|
|
else if (iter >= max_iter && result->status[0] == '\0')
|
|
snprintf(result->status, sizeof(result->status), "max_iterations");
|
|
|
|
/* Cleanup */
|
|
od_free(residuals);
|
|
od_free(jacobian);
|
|
od_free(dx);
|
|
od_free(sv);
|
|
od_free(work);
|
|
|
|
return 0;
|
|
}
|