warehack.ing/pg_orrery
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
pg_orrery/src/od_funcs.c
Ryan Malloy adfb6949e1 Add range rate fitting, weighted observations, and Gauss angles-only IOD (v0.6.0) 
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.
2026-02-17 17:48:13 -07:00

1176 lines
42 KiB
C
Raw Blame History

/*
* od_funcs.c -- SQL bindings for TLE fitting functions
*
* Exposes od_solver.c to PostgreSQL as SQL-callable functions:
* tle_from_eci() -- fit TLE from ECI ephemeris
* tle_from_topocentric() -- fit TLE from topocentric observations
* tle_fit_residuals() -- per-observation residuals diagnostic
*
* Placeholder: full implementation in Commit 3.
*/
#include "postgres.h"
#include "fmgr.h"
#include "funcapi.h"
#include "utils/timestamp.h"
#include "utils/array.h"
#include "utils/builtins.h"
#include "catalog/pg_type.h"
#include "norad.h"
#include "types.h"
#include "od_solver.h"
#include "od_math.h"
#include <math.h>
#include <string.h>
/* For construct_array() — covariance output */
#include "utils/lsyscache.h"
PG_FUNCTION_INFO_V1(tle_from_eci);
PG_FUNCTION_INFO_V1(tle_from_topocentric);
PG_FUNCTION_INFO_V1(tle_from_topocentric_multi);
PG_FUNCTION_INFO_V1(tle_fit_residuals);
PG_FUNCTION_INFO_V1(tle_from_angles);
PG_FUNCTION_INFO_V1(tle_from_angles_multi);
/* ================================================================
* Helper: pg_tle ↔ tle_t conversion (local copy, avoids symbol coupling)
* ================================================================
*/
static void
pg_tle_to_sat_code_od(const pg_tle *src, tle_t *dst)
{
memset(dst, 0, sizeof(tle_t));
dst->epoch = src->epoch;
dst->xincl = src->inclination;
dst->xnodeo = src->raan;
dst->eo = src->eccentricity;
dst->omegao = src->arg_perigee;
dst->xmo = src->mean_anomaly;
dst->xno = src->mean_motion;
dst->xndt2o = src->mean_motion_dot;
dst->xndd6o = src->mean_motion_ddot;
dst->bstar = src->bstar;
dst->norad_number = src->norad_id;
dst->bulletin_number = src->elset_num;
dst->revolution_number = src->rev_num;
dst->classification = src->classification;
dst->ephemeris_type = src->ephemeris_type;
memcpy(dst->intl_desig, src->intl_desig, 9);
}
static void
sat_code_to_pg_tle(const tle_t *src, pg_tle *dst)
{
memset(dst, 0, sizeof(pg_tle));
dst->epoch = src->epoch;
dst->inclination = src->xincl;
dst->raan = src->xnodeo;
dst->eccentricity = src->eo;
dst->arg_perigee = src->omegao;
dst->mean_anomaly = src->xmo;
dst->mean_motion = src->xno;
dst->mean_motion_dot = src->xndt2o;
dst->mean_motion_ddot = src->xndd6o;
dst->bstar = src->bstar;
dst->norad_id = src->norad_number;
dst->elset_num = src->bulletin_number;
dst->rev_num = src->revolution_number;
dst->classification = src->classification;
dst->ephemeris_type = src->ephemeris_type;
memcpy(dst->intl_desig, src->intl_desig, 9);
}
/* ================================================================
* tle_from_eci(eci_position[], timestamptz[], tle, boolean, int4)
* -> RECORD (fitted_tle tle, iterations int4, rms_final float8,
* rms_initial float8, status text)
*
* Fit a TLE from an array of ECI position/velocity observations.
* ================================================================
*/
Datum
tle_from_eci(PG_FUNCTION_ARGS)
{
ArrayType *pos_arr = PG_GETARG_ARRAYTYPE_P(0);
ArrayType *time_arr = PG_GETARG_ARRAYTYPE_P(1);
bool has_seed = !PG_ARGISNULL(2);
pg_tle *seed_pg = has_seed ? (pg_tle *) PG_GETARG_POINTER(2) : NULL;
bool fit_bstar = PG_ARGISNULL(3) ? false : PG_GETARG_BOOL(3);
int32 max_iter = PG_ARGISNULL(4) ? 15 : PG_GETARG_INT32(4);
bool has_weights = !PG_ARGISNULL(5);
int n_pos, n_times;
Datum *pos_datums, *time_datums;
bool *pos_nulls, *time_nulls;
od_observation_t *obs;
od_config_t config;
od_result_t result;
tle_t seed_sat;
int i, rc;
TupleDesc tupdesc;
Datum values[8];
bool nulls[8];
HeapTuple tuple;
/* Deconstruct arrays.
* pg_eci is 48 bytes = pass-by-reference (byval=false).
* timestamptz is int64 = pass-by-value on 64-bit. */
deconstruct_array(pos_arr, pos_arr->elemtype, sizeof(pg_eci), false,
TYPALIGN_DOUBLE, &pos_datums, &pos_nulls, &n_pos);
deconstruct_array(time_arr, TIMESTAMPTZOID, sizeof(int64), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE, &time_datums, &time_nulls, &n_times);
if (n_pos != n_times)
ereport(ERROR,
(errcode(ERRCODE_ARRAY_SUBSCRIPT_ERROR),
errmsg("positions and times arrays must have same length: "
"%d vs %d", n_pos, n_times)));
if (n_pos < 6)
ereport(ERROR,
(errcode(ERRCODE_INVALID_PARAMETER_VALUE),
errmsg("at least 6 observations required, got %d", n_pos)));
/* Build observation array */
obs = (od_observation_t *) palloc(sizeof(od_observation_t) * n_pos);
for (i = 0; i < n_pos; i++)
{
pg_eci *eci = (pg_eci *) DatumGetPointer(pos_datums[i]);
int64 ts = DatumGetInt64(time_datums[i]);
double jd = PG_EPOCH_JD + ((double)ts / (double)USECS_PER_DAY);
obs[i].jd = jd;
obs[i].data[0] = eci->x;
obs[i].data[1] = eci->y;
obs[i].data[2] = eci->z;
obs[i].data[3] = eci->vx; /* already km/s */
obs[i].data[4] = eci->vy;
obs[i].data[5] = eci->vz;
}
/* Configure solver */
memset(&config, 0, sizeof(config));
config.obs_type = OD_OBS_ECI;
config.fit_bstar = fit_bstar ? 1 : 0;
config.max_iter = max_iter;
config.observers = NULL;
config.n_observers = 0;
/* Extract per-observation weights if provided */
if (has_weights)
{
ArrayType *wt_arr = PG_GETARG_ARRAYTYPE_P(5);
Datum *wt_datums;
bool *wt_nulls;
int n_wt;
deconstruct_array(wt_arr, FLOAT8OID, sizeof(float8), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE, &wt_datums, &wt_nulls, &n_wt);
if (n_wt != n_pos)
ereport(ERROR,
(errcode(ERRCODE_ARRAY_SUBSCRIPT_ERROR),
errmsg("weights array length must match observations: "
"%d vs %d", n_wt, n_pos)));
config.weights = (double *) palloc(sizeof(double) * n_wt);
for (i = 0; i < n_wt; i++)
config.weights[i] = DatumGetFloat8(wt_datums[i]);
config.n_weights = n_wt;
}
/* Convert seed TLE if provided */
if (has_seed)
pg_tle_to_sat_code_od(seed_pg, &seed_sat);
memset(&result, 0, sizeof(result));
/* Run solver */
rc = od_fit_tle(obs, n_pos, has_seed ? &seed_sat : NULL, &config, &result);
pfree(obs);
if (config.weights)
pfree(config.weights);
if (rc != 0)
ereport(ERROR,
(errcode(ERRCODE_DATA_EXCEPTION),
errmsg("TLE fitting failed: %s", result.status)));
/* Build result tuple */
if (get_call_result_type(fcinfo, NULL, &tupdesc) != TYPEFUNC_COMPOSITE)
ereport(ERROR,
(errcode(ERRCODE_FEATURE_NOT_SUPPORTED),
errmsg("function returning record called in context "
"that cannot accept type record")));
tupdesc = BlessTupleDesc(tupdesc);
memset(nulls, 0, sizeof(nulls));
{
pg_tle *fitted = (pg_tle *) palloc(sizeof(pg_tle));
sat_code_to_pg_tle(&result.fitted_tle, fitted);
values[0] = PointerGetDatum(fitted);
}
values[1] = Int32GetDatum(result.iterations);
values[2] = Float8GetDatum(result.rms_final);
values[3] = Float8GetDatum(result.rms_initial);
values[4] = CStringGetTextDatum(result.status);
values[5] = Float8GetDatum(result.condition_number);
/* Covariance: float8[] or NULL */
if (result.cov_size > 0)
{
Datum *cov_datums = (Datum *) palloc(sizeof(Datum) * result.cov_size);
int ci;
for (ci = 0; ci < result.cov_size; ci++)
cov_datums[ci] = Float8GetDatum(result.covariance[ci]);
values[6] = PointerGetDatum(
construct_array(cov_datums, result.cov_size,
FLOAT8OID, sizeof(float8), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE));
}
else
{
nulls[6] = true;
}
values[7] = Int32GetDatum(result.cov_size > 0
? (result.cov_size == 28 ? 7 : 6)
: 0);
tuple = heap_form_tuple(tupdesc, values, nulls);
PG_RETURN_DATUM(HeapTupleGetDatum(tuple));
}
/* ================================================================
* tle_from_topocentric(topocentric[], timestamptz[], observer,
* tle, boolean, int4)
* -> RECORD (same as tle_from_eci)
* ================================================================
*/
Datum
tle_from_topocentric(PG_FUNCTION_ARGS)
{
ArrayType *topo_arr = PG_GETARG_ARRAYTYPE_P(0);
ArrayType *time_arr = PG_GETARG_ARRAYTYPE_P(1);
pg_observer *obs_pg = (pg_observer *) PG_GETARG_POINTER(2);
bool has_seed = !PG_ARGISNULL(3);
pg_tle *seed_pg = has_seed ? (pg_tle *) PG_GETARG_POINTER(3) : NULL;
bool fit_bstar = PG_ARGISNULL(4) ? false : PG_GETARG_BOOL(4);
int32 max_iter = PG_ARGISNULL(5) ? 15 : PG_GETARG_INT32(5);
bool fit_range_rate = PG_ARGISNULL(6) ? false : PG_GETARG_BOOL(6);
bool has_weights = !PG_ARGISNULL(7);
int n_topo, n_times;
od_observation_t *obs;
od_config_t config;
od_observer_t observer;
od_result_t result;
tle_t seed_sat;
int i, rc;
TupleDesc tupdesc;
Datum values[8];
bool nulls[8];
HeapTuple tuple;
/* Build observer */
observer.lat = obs_pg->lat;
observer.lon = obs_pg->lon;
observer.alt_m = obs_pg->alt_m;
od_observer_to_ecef(observer.lat, observer.lon, observer.alt_m,
observer.ecef);
/* Deconstruct arrays */
{
Datum *topo_datums;
bool *topo_nulls;
Datum *time_datums;
bool *time_nulls;
deconstruct_array(topo_arr, topo_arr->elemtype, sizeof(pg_topocentric),
false, TYPALIGN_DOUBLE,
&topo_datums, &topo_nulls, &n_topo);
deconstruct_array(time_arr, TIMESTAMPTZOID, sizeof(int64), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE, &time_datums, &time_nulls, &n_times);
if (n_topo != n_times)
ereport(ERROR,
(errcode(ERRCODE_ARRAY_SUBSCRIPT_ERROR),
errmsg("observations and times arrays must have same length: "
"%d vs %d", n_topo, n_times)));
if (n_topo < 6)
ereport(ERROR,
(errcode(ERRCODE_INVALID_PARAMETER_VALUE),
errmsg("at least 6 observations required, got %d", n_topo)));
obs = (od_observation_t *) palloc(sizeof(od_observation_t) * n_topo);
for (i = 0; i < n_topo; i++)
{
pg_topocentric *topo = (pg_topocentric *) DatumGetPointer(topo_datums[i]);
int64 ts = DatumGetInt64(time_datums[i]);
double jd = PG_EPOCH_JD + ((double)ts / (double)USECS_PER_DAY);
obs[i].jd = jd;
obs[i].data[0] = topo->azimuth;
obs[i].data[1] = topo->elevation;
obs[i].data[2] = topo->range_km;
obs[i].data[3] = topo->range_rate;
obs[i].observer_idx = 0; /* single observer */
}
}
/* Configure solver */
memset(&config, 0, sizeof(config));
config.obs_type = OD_OBS_TOPO;
config.fit_bstar = fit_bstar ? 1 : 0;
config.fit_range_rate = fit_range_rate ? 1 : 0;
config.max_iter = max_iter;
config.observers = &observer;
config.n_observers = 1;
/* Extract per-observation weights if provided */
if (has_weights)
{
ArrayType *wt_arr = PG_GETARG_ARRAYTYPE_P(7);
Datum *wt_datums;
bool *wt_nulls;
int n_wt;
deconstruct_array(wt_arr, FLOAT8OID, sizeof(float8), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE, &wt_datums, &wt_nulls, &n_wt);
if (n_wt != n_topo)
ereport(ERROR,
(errcode(ERRCODE_ARRAY_SUBSCRIPT_ERROR),
errmsg("weights array length must match observations: "
"%d vs %d", n_wt, n_topo)));
config.weights = (double *) palloc(sizeof(double) * n_wt);
for (i = 0; i < n_wt; i++)
config.weights[i] = DatumGetFloat8(wt_datums[i]);
config.n_weights = n_wt;
}
if (has_seed)
pg_tle_to_sat_code_od(seed_pg, &seed_sat);
memset(&result, 0, sizeof(result));
rc = od_fit_tle(obs, n_topo, has_seed ? &seed_sat : NULL, &config, &result);
pfree(obs);
if (config.weights)
pfree(config.weights);
if (rc != 0)
ereport(ERROR,
(errcode(ERRCODE_DATA_EXCEPTION),
errmsg("TLE fitting failed: %s", result.status)));
/* Build result tuple */
if (get_call_result_type(fcinfo, NULL, &tupdesc) != TYPEFUNC_COMPOSITE)
ereport(ERROR,
(errcode(ERRCODE_FEATURE_NOT_SUPPORTED),
errmsg("function returning record called in context "
"that cannot accept type record")));
tupdesc = BlessTupleDesc(tupdesc);
memset(nulls, 0, sizeof(nulls));
{
pg_tle *fitted = (pg_tle *) palloc(sizeof(pg_tle));
sat_code_to_pg_tle(&result.fitted_tle, fitted);
values[0] = PointerGetDatum(fitted);
}
values[1] = Int32GetDatum(result.iterations);
values[2] = Float8GetDatum(result.rms_final);
values[3] = Float8GetDatum(result.rms_initial);
values[4] = CStringGetTextDatum(result.status);
values[5] = Float8GetDatum(result.condition_number);
if (result.cov_size > 0)
{
Datum *cov_datums = (Datum *) palloc(sizeof(Datum) * result.cov_size);
int ci;
for (ci = 0; ci < result.cov_size; ci++)
cov_datums[ci] = Float8GetDatum(result.covariance[ci]);
values[6] = PointerGetDatum(
construct_array(cov_datums, result.cov_size,
FLOAT8OID, sizeof(float8), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE));
}
else
{
nulls[6] = true;
}
values[7] = Int32GetDatum(result.cov_size > 0
? (result.cov_size == 28 ? 7 : 6)
: 0);
tuple = heap_form_tuple(tupdesc, values, nulls);
PG_RETURN_DATUM(HeapTupleGetDatum(tuple));
}
/* ================================================================
* tle_from_topocentric_multi(topocentric[], timestamptz[],
* observer[], int4[],
* tle, boolean, int4)
* -> RECORD (same as tle_from_eci)
*
* Multi-observer variant: observations from different ground stations.
* observer_ids[i] indexes into the observers[] array.
* ================================================================
*/
Datum
tle_from_topocentric_multi(PG_FUNCTION_ARGS)
{
ArrayType *topo_arr = PG_GETARG_ARRAYTYPE_P(0);
ArrayType *time_arr = PG_GETARG_ARRAYTYPE_P(1);
ArrayType *obs_arr = PG_GETARG_ARRAYTYPE_P(2);
ArrayType *id_arr = PG_GETARG_ARRAYTYPE_P(3);
bool has_seed = !PG_ARGISNULL(4);
pg_tle *seed_pg = has_seed ? (pg_tle *) PG_GETARG_POINTER(4) : NULL;
bool fit_bstar = PG_ARGISNULL(5) ? false : PG_GETARG_BOOL(5);
int32 max_iter = PG_ARGISNULL(6) ? 15 : PG_GETARG_INT32(6);
bool fit_range_rate = PG_ARGISNULL(7) ? false : PG_GETARG_BOOL(7);
bool has_weights = !PG_ARGISNULL(8);
int n_topo, n_times, n_obs_sites, n_ids;
od_observation_t *obs;
od_config_t config;
od_observer_t *observers;
od_result_t result;
tle_t seed_sat;
int i, rc;
TupleDesc tupdesc;
Datum values[8];
bool nulls[8];
HeapTuple tuple;
/* Deconstruct all arrays */
Datum *topo_datums, *time_datums, *obs_datums, *id_datums;
bool *topo_nulls, *time_nulls, *obs_nulls, *id_nulls;
deconstruct_array(topo_arr, topo_arr->elemtype, sizeof(pg_topocentric),
false, TYPALIGN_DOUBLE,
&topo_datums, &topo_nulls, &n_topo);
deconstruct_array(time_arr, TIMESTAMPTZOID, sizeof(int64), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE, &time_datums, &time_nulls, &n_times);
deconstruct_array(obs_arr, obs_arr->elemtype, sizeof(pg_observer),
false, TYPALIGN_DOUBLE,
&obs_datums, &obs_nulls, &n_obs_sites);
deconstruct_array(id_arr, INT4OID, sizeof(int32), true,
TYPALIGN_INT, &id_datums, &id_nulls, &n_ids);
if (n_topo != n_times)
ereport(ERROR,
(errcode(ERRCODE_ARRAY_SUBSCRIPT_ERROR),
errmsg("observations and times arrays must have same length: "
"%d vs %d", n_topo, n_times)));
if (n_topo != n_ids)
ereport(ERROR,
(errcode(ERRCODE_ARRAY_SUBSCRIPT_ERROR),
errmsg("observations and observer_ids arrays must have same length: "
"%d vs %d", n_topo, n_ids)));
if (n_topo < 6)
ereport(ERROR,
(errcode(ERRCODE_INVALID_PARAMETER_VALUE),
errmsg("at least 6 observations required, got %d", n_topo)));
if (n_obs_sites < 1)
ereport(ERROR,
(errcode(ERRCODE_INVALID_PARAMETER_VALUE),
errmsg("at least 1 observer required")));
/* Build observer array (pre-compute ECEF for each) */
observers = (od_observer_t *) palloc(sizeof(od_observer_t) * n_obs_sites);
for (i = 0; i < n_obs_sites; i++)
{
pg_observer *op = (pg_observer *) DatumGetPointer(obs_datums[i]);
observers[i].lat = op->lat;
observers[i].lon = op->lon;
observers[i].alt_m = op->alt_m;
od_observer_to_ecef(op->lat, op->lon, op->alt_m, observers[i].ecef);
}
/* Build observation array with per-observation observer index */
obs = (od_observation_t *) palloc(sizeof(od_observation_t) * n_topo);
for (i = 0; i < n_topo; i++)
{
pg_topocentric *topo = (pg_topocentric *) DatumGetPointer(topo_datums[i]);
int64 ts = DatumGetInt64(time_datums[i]);
int32 oid = DatumGetInt32(id_datums[i]);
if (oid < 0 || oid >= n_obs_sites)
ereport(ERROR,
(errcode(ERRCODE_INVALID_PARAMETER_VALUE),
errmsg("observer_id %d out of range [0, %d)",
oid, n_obs_sites)));
obs[i].jd = PG_EPOCH_JD + ((double)ts / (double)USECS_PER_DAY);
obs[i].data[0] = topo->azimuth;
obs[i].data[1] = topo->elevation;
obs[i].data[2] = topo->range_km;
obs[i].data[3] = topo->range_rate;
obs[i].observer_idx = oid;
}
/* Configure solver */
memset(&config, 0, sizeof(config));
config.obs_type = OD_OBS_TOPO;
config.fit_bstar = fit_bstar ? 1 : 0;
config.fit_range_rate = fit_range_rate ? 1 : 0;
config.max_iter = max_iter;
config.observers = observers;
config.n_observers = n_obs_sites;
/* Extract per-observation weights if provided */
if (has_weights)
{
ArrayType *wt_arr = PG_GETARG_ARRAYTYPE_P(8);
Datum *wt_datums;
bool *wt_nulls;
int n_wt;
deconstruct_array(wt_arr, FLOAT8OID, sizeof(float8), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE, &wt_datums, &wt_nulls, &n_wt);
if (n_wt != n_topo)
ereport(ERROR,
(errcode(ERRCODE_ARRAY_SUBSCRIPT_ERROR),
errmsg("weights array length must match observations: "
"%d vs %d", n_wt, n_topo)));
config.weights = (double *) palloc(sizeof(double) * n_wt);
for (i = 0; i < n_wt; i++)
config.weights[i] = DatumGetFloat8(wt_datums[i]);
config.n_weights = n_wt;
}
if (has_seed)
pg_tle_to_sat_code_od(seed_pg, &seed_sat);
memset(&result, 0, sizeof(result));
rc = od_fit_tle(obs, n_topo, has_seed ? &seed_sat : NULL, &config, &result);
pfree(obs);
pfree(observers);
if (config.weights)
pfree(config.weights);
if (rc != 0)
ereport(ERROR,
(errcode(ERRCODE_DATA_EXCEPTION),
errmsg("TLE fitting failed: %s", result.status)));
/* Build result tuple */
if (get_call_result_type(fcinfo, NULL, &tupdesc) != TYPEFUNC_COMPOSITE)
ereport(ERROR,
(errcode(ERRCODE_FEATURE_NOT_SUPPORTED),
errmsg("function returning record called in context "
"that cannot accept type record")));
tupdesc = BlessTupleDesc(tupdesc);
memset(nulls, 0, sizeof(nulls));
{
pg_tle *fitted = (pg_tle *) palloc(sizeof(pg_tle));
sat_code_to_pg_tle(&result.fitted_tle, fitted);
values[0] = PointerGetDatum(fitted);
}
values[1] = Int32GetDatum(result.iterations);
values[2] = Float8GetDatum(result.rms_final);
values[3] = Float8GetDatum(result.rms_initial);
values[4] = CStringGetTextDatum(result.status);
values[5] = Float8GetDatum(result.condition_number);
if (result.cov_size > 0)
{
Datum *cov_datums = (Datum *) palloc(sizeof(Datum) * result.cov_size);
int ci;
for (ci = 0; ci < result.cov_size; ci++)
cov_datums[ci] = Float8GetDatum(result.covariance[ci]);
values[6] = PointerGetDatum(
construct_array(cov_datums, result.cov_size,
FLOAT8OID, sizeof(float8), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE));
}
else
{
nulls[6] = true;
}
values[7] = Int32GetDatum(result.cov_size > 0
? (result.cov_size == 28 ? 7 : 6)
: 0);
tuple = heap_form_tuple(tupdesc, values, nulls);
PG_RETURN_DATUM(HeapTupleGetDatum(tuple));
}
/* ================================================================
* tle_from_angles(ra_hours[], dec_degrees[], timestamptz[], observer,
* tle, boolean, int4, float8[])
* -> RECORD (same 8-column output as tle_from_eci)
*
* Fit TLE from angles-only (RA/Dec) observations.
* RA in hours [0,24), Dec in degrees [-90,90].
* Uses Gauss IOD for seed-free initial guess.
* ================================================================
*/
Datum
tle_from_angles(PG_FUNCTION_ARGS)
{
ArrayType *ra_arr = PG_GETARG_ARRAYTYPE_P(0);
ArrayType *dec_arr = PG_GETARG_ARRAYTYPE_P(1);
ArrayType *time_arr = PG_GETARG_ARRAYTYPE_P(2);
pg_observer *obs_pg = (pg_observer *) PG_GETARG_POINTER(3);
bool has_seed = !PG_ARGISNULL(4);
pg_tle *seed_pg = has_seed ? (pg_tle *) PG_GETARG_POINTER(4) : NULL;
bool fit_bstar = PG_ARGISNULL(5) ? false : PG_GETARG_BOOL(5);
int32 max_iter = PG_ARGISNULL(6) ? 15 : PG_GETARG_INT32(6);
bool has_weights = !PG_ARGISNULL(7);
int n_ra, n_dec, n_times;
Datum *ra_datums, *dec_datums, *time_datums;
bool *ra_nulls, *dec_nulls, *time_nulls;
od_observation_t *obs;
od_config_t config;
od_observer_t observer;
od_result_t result;
tle_t seed_sat;
int i, rc;
TupleDesc tupdesc;
Datum values[8];
bool nulls[8];
HeapTuple tuple;
/* Build observer */
observer.lat = obs_pg->lat;
observer.lon = obs_pg->lon;
observer.alt_m = obs_pg->alt_m;
od_observer_to_ecef(observer.lat, observer.lon, observer.alt_m,
observer.ecef);
/* Deconstruct arrays */
deconstruct_array(ra_arr, FLOAT8OID, sizeof(float8), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE, &ra_datums, &ra_nulls, &n_ra);
deconstruct_array(dec_arr, FLOAT8OID, sizeof(float8), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE, &dec_datums, &dec_nulls, &n_dec);
deconstruct_array(time_arr, TIMESTAMPTZOID, sizeof(int64), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE, &time_datums, &time_nulls, &n_times);
if (n_ra != n_dec || n_ra != n_times)
ereport(ERROR,
(errcode(ERRCODE_ARRAY_SUBSCRIPT_ERROR),
errmsg("ra, dec, and times arrays must have same length: "
"%d, %d, %d", n_ra, n_dec, n_times)));
if (n_ra < 6)
ereport(ERROR,
(errcode(ERRCODE_INVALID_PARAMETER_VALUE),
errmsg("at least 6 observations required, got %d", n_ra)));
/* Build observation array — convert RA hours → radians, Dec deg → radians */
obs = (od_observation_t *) palloc(sizeof(od_observation_t) * n_ra);
for (i = 0; i < n_ra; i++)
{
double ra_hr = DatumGetFloat8(ra_datums[i]);
double dec_dg = DatumGetFloat8(dec_datums[i]);
int64 ts = DatumGetInt64(time_datums[i]);
if (ra_hr < 0.0 || ra_hr >= 24.0)
ereport(ERROR,
(errcode(ERRCODE_INVALID_PARAMETER_VALUE),
errmsg("RA must be in [0, 24) hours, got %g", ra_hr)));
if (dec_dg < -90.0 || dec_dg > 90.0)
ereport(ERROR,
(errcode(ERRCODE_INVALID_PARAMETER_VALUE),
errmsg("Dec must be in [-90, 90] degrees, got %g", dec_dg)));
obs[i].jd = PG_EPOCH_JD + ((double)ts / (double)USECS_PER_DAY);
obs[i].data[0] = ra_hr * (M_PI / 12.0); /* hours → radians */
obs[i].data[1] = dec_dg * (M_PI / 180.0); /* degrees → radians */
obs[i].observer_idx = 0;
}
/* Configure solver */
memset(&config, 0, sizeof(config));
config.obs_type = OD_OBS_RADEC;
config.fit_bstar = fit_bstar ? 1 : 0;
config.max_iter = max_iter;
config.observers = &observer;
config.n_observers = 1;
/* Extract per-observation weights if provided */
if (has_weights)
{
ArrayType *wt_arr = PG_GETARG_ARRAYTYPE_P(7);
Datum *wt_datums;
bool *wt_nulls;
int n_wt;
deconstruct_array(wt_arr, FLOAT8OID, sizeof(float8), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE, &wt_datums, &wt_nulls, &n_wt);
if (n_wt != n_ra)
ereport(ERROR,
(errcode(ERRCODE_ARRAY_SUBSCRIPT_ERROR),
errmsg("weights array length must match observations: "
"%d vs %d", n_wt, n_ra)));
config.weights = (double *) palloc(sizeof(double) * n_wt);
for (i = 0; i < n_wt; i++)
config.weights[i] = DatumGetFloat8(wt_datums[i]);
config.n_weights = n_wt;
}
if (has_seed)
pg_tle_to_sat_code_od(seed_pg, &seed_sat);
memset(&result, 0, sizeof(result));
rc = od_fit_tle(obs, n_ra, has_seed ? &seed_sat : NULL, &config, &result);
pfree(obs);
if (config.weights)
pfree(config.weights);
if (rc != 0)
ereport(ERROR,
(errcode(ERRCODE_DATA_EXCEPTION),
errmsg("TLE fitting failed: %s", result.status)));
/* Build result tuple */
if (get_call_result_type(fcinfo, NULL, &tupdesc) != TYPEFUNC_COMPOSITE)
ereport(ERROR,
(errcode(ERRCODE_FEATURE_NOT_SUPPORTED),
errmsg("function returning record called in context "
"that cannot accept type record")));
tupdesc = BlessTupleDesc(tupdesc);
memset(nulls, 0, sizeof(nulls));
{
pg_tle *fitted = (pg_tle *) palloc(sizeof(pg_tle));
sat_code_to_pg_tle(&result.fitted_tle, fitted);
values[0] = PointerGetDatum(fitted);
}
values[1] = Int32GetDatum(result.iterations);
values[2] = Float8GetDatum(result.rms_final);
values[3] = Float8GetDatum(result.rms_initial);
values[4] = CStringGetTextDatum(result.status);
values[5] = Float8GetDatum(result.condition_number);
if (result.cov_size > 0)
{
Datum *cov_datums = (Datum *) palloc(sizeof(Datum) * result.cov_size);
int ci;
for (ci = 0; ci < result.cov_size; ci++)
cov_datums[ci] = Float8GetDatum(result.covariance[ci]);
values[6] = PointerGetDatum(
construct_array(cov_datums, result.cov_size,
FLOAT8OID, sizeof(float8), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE));
}
else
{
nulls[6] = true;
}
values[7] = Int32GetDatum(result.cov_size > 0
? (result.cov_size == 28 ? 7 : 6)
: 0);
tuple = heap_form_tuple(tupdesc, values, nulls);
PG_RETURN_DATUM(HeapTupleGetDatum(tuple));
}
/* ================================================================
* tle_from_angles_multi(ra_hours[], dec_degrees[], timestamptz[],
* observer[], int4[],
* tle, boolean, int4, float8[])
* -> RECORD (same 8-column output)
*
* Multi-observer variant of tle_from_angles.
* ================================================================
*/
Datum
tle_from_angles_multi(PG_FUNCTION_ARGS)
{
ArrayType *ra_arr = PG_GETARG_ARRAYTYPE_P(0);
ArrayType *dec_arr = PG_GETARG_ARRAYTYPE_P(1);
ArrayType *time_arr = PG_GETARG_ARRAYTYPE_P(2);
ArrayType *obs_arr = PG_GETARG_ARRAYTYPE_P(3);
ArrayType *id_arr = PG_GETARG_ARRAYTYPE_P(4);
bool has_seed = !PG_ARGISNULL(5);
pg_tle *seed_pg = has_seed ? (pg_tle *) PG_GETARG_POINTER(5) : NULL;
bool fit_bstar = PG_ARGISNULL(6) ? false : PG_GETARG_BOOL(6);
int32 max_iter = PG_ARGISNULL(7) ? 15 : PG_GETARG_INT32(7);
bool has_weights = !PG_ARGISNULL(8);
int n_ra, n_dec, n_times, n_obs_sites, n_ids;
Datum *ra_datums, *dec_datums, *time_datums, *obs_datums, *id_datums;
bool *ra_nulls, *dec_nulls, *time_nulls, *obs_nulls, *id_nulls;
od_observation_t *obs;
od_config_t config;
od_observer_t *observers;
od_result_t result;
tle_t seed_sat;
int i, rc;
TupleDesc tupdesc;
Datum values[8];
bool nulls[8];
HeapTuple tuple;
/* Deconstruct all arrays */
deconstruct_array(ra_arr, FLOAT8OID, sizeof(float8), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE, &ra_datums, &ra_nulls, &n_ra);
deconstruct_array(dec_arr, FLOAT8OID, sizeof(float8), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE, &dec_datums, &dec_nulls, &n_dec);
deconstruct_array(time_arr, TIMESTAMPTZOID, sizeof(int64), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE, &time_datums, &time_nulls, &n_times);
deconstruct_array(obs_arr, obs_arr->elemtype, sizeof(pg_observer),
false, TYPALIGN_DOUBLE,
&obs_datums, &obs_nulls, &n_obs_sites);
deconstruct_array(id_arr, INT4OID, sizeof(int32), true,
TYPALIGN_INT, &id_datums, &id_nulls, &n_ids);
if (n_ra != n_dec || n_ra != n_times)
ereport(ERROR,
(errcode(ERRCODE_ARRAY_SUBSCRIPT_ERROR),
errmsg("ra, dec, and times arrays must have same length: "
"%d, %d, %d", n_ra, n_dec, n_times)));
if (n_ra != n_ids)
ereport(ERROR,
(errcode(ERRCODE_ARRAY_SUBSCRIPT_ERROR),
errmsg("observations and observer_ids arrays must have same length: "
"%d vs %d", n_ra, n_ids)));
if (n_ra < 6)
ereport(ERROR,
(errcode(ERRCODE_INVALID_PARAMETER_VALUE),
errmsg("at least 6 observations required, got %d", n_ra)));
if (n_obs_sites < 1)
ereport(ERROR,
(errcode(ERRCODE_INVALID_PARAMETER_VALUE),
errmsg("at least 1 observer required")));
/* Build observer array */
observers = (od_observer_t *) palloc(sizeof(od_observer_t) * n_obs_sites);
for (i = 0; i < n_obs_sites; i++)
{
pg_observer *op = (pg_observer *) DatumGetPointer(obs_datums[i]);
observers[i].lat = op->lat;
observers[i].lon = op->lon;
observers[i].alt_m = op->alt_m;
od_observer_to_ecef(op->lat, op->lon, op->alt_m, observers[i].ecef);
}
/* Build observation array */
obs = (od_observation_t *) palloc(sizeof(od_observation_t) * n_ra);
for (i = 0; i < n_ra; i++)
{
double ra_hr = DatumGetFloat8(ra_datums[i]);
double dec_dg = DatumGetFloat8(dec_datums[i]);
int64 ts = DatumGetInt64(time_datums[i]);
int32 oid = DatumGetInt32(id_datums[i]);
if (ra_hr < 0.0 || ra_hr >= 24.0)
ereport(ERROR,
(errcode(ERRCODE_INVALID_PARAMETER_VALUE),
errmsg("RA must be in [0, 24) hours, got %g", ra_hr)));
if (dec_dg < -90.0 || dec_dg > 90.0)
ereport(ERROR,
(errcode(ERRCODE_INVALID_PARAMETER_VALUE),
errmsg("Dec must be in [-90, 90] degrees, got %g", dec_dg)));
if (oid < 0 || oid >= n_obs_sites)
ereport(ERROR,
(errcode(ERRCODE_INVALID_PARAMETER_VALUE),
errmsg("observer_id %d out of range [0, %d)",
oid, n_obs_sites)));
obs[i].jd = PG_EPOCH_JD + ((double)ts / (double)USECS_PER_DAY);
obs[i].data[0] = ra_hr * (M_PI / 12.0);
obs[i].data[1] = dec_dg * (M_PI / 180.0);
obs[i].observer_idx = oid;
}
/* Configure solver */
memset(&config, 0, sizeof(config));
config.obs_type = OD_OBS_RADEC;
config.fit_bstar = fit_bstar ? 1 : 0;
config.max_iter = max_iter;
config.observers = observers;
config.n_observers = n_obs_sites;
/* Extract per-observation weights if provided */
if (has_weights)
{
ArrayType *wt_arr = PG_GETARG_ARRAYTYPE_P(8);
Datum *wt_datums;
bool *wt_nulls;
int n_wt;
deconstruct_array(wt_arr, FLOAT8OID, sizeof(float8), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE, &wt_datums, &wt_nulls, &n_wt);
if (n_wt != n_ra)
ereport(ERROR,
(errcode(ERRCODE_ARRAY_SUBSCRIPT_ERROR),
errmsg("weights array length must match observations: "
"%d vs %d", n_wt, n_ra)));
config.weights = (double *) palloc(sizeof(double) * n_wt);
for (i = 0; i < n_wt; i++)
config.weights[i] = DatumGetFloat8(wt_datums[i]);
config.n_weights = n_wt;
}
if (has_seed)
pg_tle_to_sat_code_od(seed_pg, &seed_sat);
memset(&result, 0, sizeof(result));
rc = od_fit_tle(obs, n_ra, has_seed ? &seed_sat : NULL, &config, &result);
pfree(obs);
pfree(observers);
if (config.weights)
pfree(config.weights);
if (rc != 0)
ereport(ERROR,
(errcode(ERRCODE_DATA_EXCEPTION),
errmsg("TLE fitting failed: %s", result.status)));
/* Build result tuple */
if (get_call_result_type(fcinfo, NULL, &tupdesc) != TYPEFUNC_COMPOSITE)
ereport(ERROR,
(errcode(ERRCODE_FEATURE_NOT_SUPPORTED),
errmsg("function returning record called in context "
"that cannot accept type record")));
tupdesc = BlessTupleDesc(tupdesc);
memset(nulls, 0, sizeof(nulls));
{
pg_tle *fitted = (pg_tle *) palloc(sizeof(pg_tle));
sat_code_to_pg_tle(&result.fitted_tle, fitted);
values[0] = PointerGetDatum(fitted);
}
values[1] = Int32GetDatum(result.iterations);
values[2] = Float8GetDatum(result.rms_final);
values[3] = Float8GetDatum(result.rms_initial);
values[4] = CStringGetTextDatum(result.status);
values[5] = Float8GetDatum(result.condition_number);
if (result.cov_size > 0)
{
Datum *cov_datums = (Datum *) palloc(sizeof(Datum) * result.cov_size);
int ci;
for (ci = 0; ci < result.cov_size; ci++)
cov_datums[ci] = Float8GetDatum(result.covariance[ci]);
values[6] = PointerGetDatum(
construct_array(cov_datums, result.cov_size,
FLOAT8OID, sizeof(float8), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE));
}
else
{
nulls[6] = true;
}
values[7] = Int32GetDatum(result.cov_size > 0
? (result.cov_size == 28 ? 7 : 6)
: 0);
tuple = heap_form_tuple(tupdesc, values, nulls);
PG_RETURN_DATUM(HeapTupleGetDatum(tuple));
}
/* ================================================================
* tle_fit_residuals(tle, eci_position[], timestamptz[])
* -> TABLE (t timestamptz, dx_km float8, dy_km float8, dz_km float8,
* pos_err_km float8)
*
* Compute per-observation residuals between a TLE and ECI observations.
* ================================================================
*/
Datum
tle_fit_residuals(PG_FUNCTION_ARGS)
{
FuncCallContext *funcctx;
if (SRF_IS_FIRSTCALL())
{
MemoryContext oldctx;
pg_tle *tle;
ArrayType *pos_arr;
ArrayType *time_arr;
int n_pos, n_times;
Datum *pos_datums;
bool *pos_nulls;
Datum *time_datums;
bool *time_nulls;
TupleDesc tupdesc;
/* Context for residual data (persists across calls) */
typedef struct {
tle_t sat;
double *params;
int is_deep;
int n_obs;
double *jds; /* Julian dates */
double *obs_pos; /* observed positions (3 * n_obs) */
} residual_ctx;
residual_ctx *ctx;
int i;
funcctx = SRF_FIRSTCALL_INIT();
oldctx = MemoryContextSwitchTo(funcctx->multi_call_memory_ctx);
tle = (pg_tle *) PG_GETARG_POINTER(0);
pos_arr = PG_GETARG_ARRAYTYPE_P(1);
time_arr = PG_GETARG_ARRAYTYPE_P(2);
deconstruct_array(pos_arr, pos_arr->elemtype, sizeof(pg_eci), false,
TYPALIGN_DOUBLE, &pos_datums, &pos_nulls, &n_pos);
deconstruct_array(time_arr, TIMESTAMPTZOID, sizeof(int64), FLOAT8PASSBYVAL,
TYPALIGN_DOUBLE, &time_datums, &time_nulls, &n_times);
if (n_pos != n_times)
ereport(ERROR,
(errcode(ERRCODE_ARRAY_SUBSCRIPT_ERROR),
errmsg("positions and times arrays must have same length")));
ctx = (residual_ctx *) palloc0(sizeof(residual_ctx));
pg_tle_to_sat_code_od(tle, &ctx->sat);
ctx->is_deep = select_ephemeris(&ctx->sat);
if (ctx->is_deep < 0)
ereport(ERROR,
(errcode(ERRCODE_DATA_EXCEPTION),
errmsg("invalid TLE for residual computation")));
ctx->params = (double *) palloc(sizeof(double) * N_SAT_PARAMS);
if (ctx->is_deep)
SDP4_init(ctx->params, &ctx->sat);
else
SGP4_init(ctx->params, &ctx->sat);
ctx->n_obs = n_pos;
ctx->jds = (double *) palloc(sizeof(double) * n_pos);
ctx->obs_pos = (double *) palloc(sizeof(double) * 3 * n_pos);
for (i = 0; i < n_pos; i++)
{
pg_eci *eci = (pg_eci *) DatumGetPointer(pos_datums[i]);
int64 ts = DatumGetInt64(time_datums[i]);
ctx->jds[i] = PG_EPOCH_JD + ((double)ts / (double)USECS_PER_DAY);
ctx->obs_pos[i * 3 + 0] = eci->x;
ctx->obs_pos[i * 3 + 1] = eci->y;
ctx->obs_pos[i * 3 + 2] = eci->z;
}
funcctx->max_calls = n_pos;
funcctx->user_fctx = ctx;
tupdesc = CreateTemplateTupleDesc(5);
TupleDescInitEntry(tupdesc, 1, "t", TIMESTAMPTZOID, -1, 0);
TupleDescInitEntry(tupdesc, 2, "dx_km", FLOAT8OID, -1, 0);
TupleDescInitEntry(tupdesc, 3, "dy_km", FLOAT8OID, -1, 0);
TupleDescInitEntry(tupdesc, 4, "dz_km", FLOAT8OID, -1, 0);
TupleDescInitEntry(tupdesc, 5, "pos_err_km", FLOAT8OID, -1, 0);
funcctx->tuple_desc = BlessTupleDesc(tupdesc);
MemoryContextSwitchTo(oldctx);
}
funcctx = SRF_PERCALL_SETUP();
if (funcctx->call_cntr < funcctx->max_calls)
{
typedef struct {
tle_t sat;
double *params;
int is_deep;
int n_obs;
double *jds;
double *obs_pos;
} residual_ctx;
residual_ctx *ctx = (residual_ctx *) funcctx->user_fctx;
int idx = funcctx->call_cntr;
double tsince, pos[3], vel[3];
double dx, dy, dz, pos_err;
int err;
int64 ts;
Datum values[5];
bool nulls[5];
HeapTuple tuple;
tsince = (ctx->jds[idx] - ctx->sat.epoch) * 1440.0;
if (ctx->is_deep)
err = SDP4(tsince, &ctx->sat, ctx->params, pos, vel);
else
err = SGP4(tsince, &ctx->sat, ctx->params, pos, vel);
dx = ctx->obs_pos[idx * 3 + 0] - pos[0];
dy = ctx->obs_pos[idx * 3 + 1] - pos[1];
dz = ctx->obs_pos[idx * 3 + 2] - pos[2];
pos_err = sqrt(dx * dx + dy * dy + dz * dz);
ts = (int64)((ctx->jds[idx] - PG_EPOCH_JD) * (double)USECS_PER_DAY);
memset(nulls, 0, sizeof(nulls));
values[0] = Int64GetDatum(ts);
values[1] = Float8GetDatum(dx);
values[2] = Float8GetDatum(dy);
values[3] = Float8GetDatum(dz);
values[4] = Float8GetDatum(pos_err);
(void)err;
tuple = heap_form_tuple(funcctx->tuple_desc, values, nulls);
SRF_RETURN_NEXT(funcctx, HeapTupleGetDatum(tuple));
}
SRF_RETURN_DONE(funcctx);
}
Reference in New Issue View Git Blame Copy Permalink