spacecruft
/
sattools
1
0
Fork 0
Code Releases Activity
sattools/src/xyz2tle.c

784 lines
16 KiB
C
Raw Permalink Blame History

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <getopt.h>
#include <ctype.h>
#include "sgdp4h.h"
#include "satutl.h"
#define LIM 80
#define NMAX 256
#define D2R M_PI/180.0
#define R2D 180.0/M_PI
#define XKE 0.07436680 // Guassian Gravitational Constant
#define XKMPER 6378.135
#define AE 1.0
#define XMNPDA 1440.0
struct data
{
int n, nsel;
struct point *p;
double chisq, rms;
} d;
struct point
{
int flag;
double mjd;
xyz_t r;
};
orbit_t orb;
void versafit (int m, int n, double *a, double *da, double (*func) (double *),
double dchisq, double tol, char *opt);
// Dot product
float
dot (xyz_t a, xyz_t b)
{
return a.x * b.x + a.y * b.y + a.z * b.z;
}
// Magnitude
double
magnitude (xyz_t a)
{
return sqrt (dot (a, a));
}
// Cross product
xyz_t
cross (xyz_t a, xyz_t b)
{
xyz_t c;
c.x = a.y * b.z - a.z * b.y;
c.y = a.z * b.x - a.x * b.z;
c.z = a.x * b.y - a.y * b.x;
return c;
}
// Return x modulo y [0,y)
double
modulo (double x, double y)
{
x = fmod (x, y);
if (x < 0.0)
x += y;
return x;
}
// Compute Julian Day from Date
double
date2mjd (int year, int month, double day)
{
int a, b;
double jd;
if (month < 3)
{
year--;
month += 12;
}
a = floor (year / 100.);
b = 2. - a + floor (a / 4.);
if (year < 1582)
b = 0;
if (year == 1582 && month < 10)
b = 0;
if (year == 1582 && month == 10 && day <= 4)
b = 0;
jd =
floor (365.25 * (year + 4716)) + floor (30.6001 * (month + 1)) + day + b -
1524.5;
return jd - 2400000.5;
}
// Read a line of maximum length int lim from file FILE into string s
int
fgetline (FILE * file, char *s, int lim)
{
int c, i = 0;
while (--lim > 0 && (c = fgetc (file)) != EOF && c != '\n')
s[i++] = c;
if (c == '\t')
c = ' ';
if (c == '\n')
s[i++] = c;
s[i] = '\0';
return i;
}
// Format TLE
void
format_tle (orbit_t orb, char *line1, char *line2)
{
int i, csum;
char sbstar[] = " 00000-0", bstar[13];
// Format Bstar term
if (fabs (orb.bstar) > 1e-9)
{
sprintf (bstar, "%11.4e", 10 * orb.bstar);
sbstar[0] = bstar[0];
sbstar[1] = bstar[1];
sbstar[2] = bstar[3];
sbstar[3] = bstar[4];
sbstar[4] = bstar[5];
sbstar[5] = bstar[6];
sbstar[6] = bstar[8];
sbstar[7] = bstar[10];
sbstar[8] = '\0';
}
// Print lines
sprintf (line1, "1 %05dU %-8s %2d%012.8f .00000000 00000-0 %8s 0 0",
orb.satno, orb.desig, orb.ep_year - 2000, orb.ep_day, sbstar);
sprintf (line2, "2 %05d %8.4f %8.4f %07.0f %8.4f %8.4f %11.8f%5ld",
orb.satno, DEG (orb.eqinc), DEG (orb.ascn), 1E7 * orb.ecc,
DEG (orb.argp), DEG (orb.mnan), orb.rev, orb.norb);
// Compute checksums
for (i = 0, csum = 0; i < strlen (line1); i++)
{
if (isdigit (line1[i]))
csum += line1[i] - '0';
else if (line1[i] == '-')
csum++;
}
sprintf (line1, "%s%d", line1, csum % 10);
for (i = 0, csum = 0; i < strlen (line2); i++)
{
if (isdigit (line2[i]))
csum += line2[i] - '0';
else if (line2[i] == '-')
csum++;
}
sprintf (line2, "%s%d", line2, csum % 10);
return;
}
// Precess a celestial position
void
precess (double mjd0, double ra0, double de0, double mjd, double *ra,
double *de)
{
double t0, t;
double zeta, z, theta;
double a, b, c;
// Angles in radians
ra0 *= D2R;
de0 *= D2R;
// Time in centuries
t0 = (mjd0 - 51544.5) / 36525.0;
t = (mjd - mjd0) / 36525.0;
// Precession angles
zeta = (2306.2181 + 1.39656 * t0 - 0.000139 * t0 * t0) * t;
zeta += (0.30188 - 0.000344 * t0) * t * t + 0.017998 * t * t * t;
zeta *= D2R / 3600.0;
z = (2306.2181 + 1.39656 * t0 - 0.000139 * t0 * t0) * t;
z += (1.09468 + 0.000066 * t0) * t * t + 0.018203 * t * t * t;
z *= D2R / 3600.0;
theta = (2004.3109 - 0.85330 * t0 - 0.000217 * t0 * t0) * t;
theta += -(0.42665 + 0.000217 * t0) * t * t - 0.041833 * t * t * t;
theta *= D2R / 3600.0;
a = cos (de0) * sin (ra0 + zeta);
b = cos (theta) * cos (de0) * cos (ra0 + zeta) - sin (theta) * sin (de0);
c = sin (theta) * cos (de0) * cos (ra0 + zeta) + cos (theta) * sin (de0);
*ra = (atan2 (a, b) + z) * R2D;
*de = asin (c) * R2D;
if (*ra < 360.0)
*ra += 360.0;
if (*ra > 360.0)
*ra -= 360.0;
return;
}
// Read data file
struct data
read_data (char *filename, double mjd0)
{
int i = 0, status;
char line[LIM];
FILE *file;
struct data d;
int min;
double ra, de, ra0, de0, r;
double x, y, z;
// Open file
file = fopen (filename, "r");
if (file == NULL)
{
fprintf (stderr, "Failed to open %s\n", filename);
exit (1);
}
// Count lines
while (fgetline (file, line, LIM) > 0)
i++;
d.n = i;
// Allocate
d.p = (struct point *) malloc (sizeof (struct point) * d.n);
// Rewind file
rewind (file);
// Read data
i = 0;
while (fgetline (file, line, LIM) > 0)
{
status = sscanf (line, "%d,%lf,%lf,%lf", &min, &x, &y, &z);
if (d.n == 1008)
min *= 10;
d.p[i].mjd = mjd0 + (double) min / 1440.0;
// Precess position
r = sqrt (x * x + y * y + z * z);
ra0 = atan2 (y, x) * R2D;
de0 = asin (z / r) * R2D;
precess (51544.5, ra0, de0, d.p[i].mjd, &ra, &de);
d.p[i].r.x = r * cos (de * D2R) * cos (ra * D2R);
d.p[i].r.y = r * cos (de * D2R) * sin (ra * D2R);
d.p[i].r.z = r * sin (de * D2R);
d.p[i].flag = 0;
i++;
}
// Close file
fclose (file);
return d;
}
// Read tle
orbit_t
read_tle (char *filename, int satno)
{
int i;
FILE *file;
orbit_t orb;
file = fopen (filename, "r");
if (file == NULL)
fatal_error ("Failed to open %s\n", filename);
// Read TLE
read_twoline (file, satno, &orb);
fclose (file);
return orb;
}
// Chi-squared
double
chisq (double *a)
{
int i, imode, n;
double chisq;
xyz_t satpos, satvel, dr;
// Construct struct
// a[0]: inclination
// a[1]: RA of ascending node
// a[2]: eccentricity
// a[3]: argument of periastron
// a[4]: mean anomaly
// a[5]: revs per day
// a[6]: bstar drag
if (a[2] < 0.0)
a[2] = 0.0;
if (a[0] < 0.0)
{
a[0] *= -1;
a[1] += 180.0;
}
else if (a[0] > 180.0)
{
a[0] = 180.0;
}
if (a[5] > 20.0)
a[5] = 20.0;
if (a[5] < 0.1)
a[5] = 0.1;
// Set parameters
orb.eqinc = RAD (a[0]);
orb.ascn = RAD (modulo (a[1], 360.0));
orb.ecc = a[2];
orb.argp = RAD (modulo (a[3], 360.0));
orb.mnan = RAD (modulo (a[4], 360.0));
orb.rev = a[5];
orb.bstar = a[6];
// Initialize
imode = init_sgdp4 (&orb);
if (imode == SGDP4_ERROR)
printf ("Error\n");
// Loop over points
for (i = 0, chisq = 0.0, n = 0; i < d.n; i++)
{
// Skip unflagged positions
if (d.p[i].flag != 1)
continue;
// Get satellite position
satpos_xyz (d.p[i].mjd + 2400000.5, &satpos, &satvel);
dr.x = (satpos.x - d.p[i].r.x);
dr.y = (satpos.y - d.p[i].r.y);
dr.z = (satpos.z - d.p[i].r.z);
// Add
chisq += dr.x * dr.x + dr.y * dr.y + dr.z * dr.z;
n++;
}
chisq /= (double) n;
d.rms = sqrt (chisq);
d.nsel = n;
return chisq;
}
double
decode_filename (char *filename, int *satno)
{
int year, month, day, hour, min, sec;
int status;
double mjd;
status =
sscanf (filename, "%6d_%4d%2d%2d_%2d%2d%2d", satno, &year, &month, &day,
&hour, &min, &sec);
mjd =
date2mjd (year, month,
(double) day + hour / 24.0 + min / 1440.0 + sec / 86400.0);
return mjd;
}
// Compute Date from Julian Day
void
mjd2date (double mjd, int *year, int *month, double *day)
{
double f, jd;
int z, alpha, a, b, c, d, e;
jd = mjd + 2400000.5;
jd += 0.5;
z = floor (jd);
f = fmod (jd, 1.);
if (z < 2299161)
a = z;
else
{
alpha = floor ((z - 1867216.25) / 36524.25);
a = z + 1 + alpha - floor (alpha / 4.);
}
b = a + 1524;
c = floor ((b - 122.1) / 365.25);
d = floor (365.25 * c);
e = floor ((b - d) / 30.6001);
*day = b - d - floor (30.6001 * e) + f;
if (e < 14)
*month = e - 1;
else
*month = e - 13;
if (*month > 2)
*year = c - 4716;
else
*year = c - 4715;
return;
}
// MJD to DOY
double
mjd2doy (double mjd, int *yr)
{
int year, month, k = 2;
double day, doy;
mjd2date (mjd, &year, &month, &day);
if (year % 4 == 0 && year % 400 != 0)
k = 1;
doy =
floor (275.0 * month / 9.0) - k * floor ((month + 9.0) / 12.0) + day - 30;
*yr = year;
return doy;
}
// Clasical elements
orbit_t
classel (int ep_year, double ep_day, xyz_t r, xyz_t v)
{
int i;
double rm, vm, vm2, rvm, mu = 1.0;;
double chi, xp, yp, sx, cx, b, ee;
double a, ecc, incl, node, peri, mm, n;
xyz_t h, e, kk, nn;
orbit_t orb;
r.x /= XKMPER;
r.y /= XKMPER;
r.z /= XKMPER;
v.x /= (XKE * XKMPER / AE * XMNPDA / 86400.0);
v.y /= (XKE * XKMPER / AE * XMNPDA / 86400.0);
v.z /= (XKE * XKMPER / AE * XMNPDA / 86400.0);
rm = magnitude (r);
vm2 = dot (v, v);
rvm = dot (r, v);
h = cross (r, v);
chi = dot (h, h) / mu;
e.x = (vm2 / mu - 1.0 / rm) * r.x - rvm / mu * v.x;
e.y = (vm2 / mu - 1.0 / rm) * r.y - rvm / mu * v.y;
e.z = (vm2 / mu - 1.0 / rm) * r.z - rvm / mu * v.z;
a = pow (2.0 / rm - vm2 / mu, -1);
ecc = magnitude (e);
incl = acos (h.z / magnitude (h)) * R2D;
kk.x = 0.0;
kk.y = 0.0;
kk.z = 1.0;
nn = cross (kk, h);
if (nn.x == 0.0 && nn.y == 0.0)
nn.x = 1.0;
node = atan2 (nn.y, nn.x) * R2D;
if (node < 0.0)
node += 360.0;
peri = acos (dot (nn, e) / (magnitude (nn) * ecc)) * R2D;
if (e.z < 0.0)
peri = 360.0 - peri;
if (peri < 0.0)
peri += 360.0;
// Elliptic motion
if (ecc < 1.0)
{
xp = (chi - rm) / ecc;
yp = rvm / ecc * sqrt (chi / mu);
b = a * sqrt (1.0 - ecc * ecc);
cx = xp / a + ecc;
sx = yp / b;
ee = atan2 (sx, cx);
n = XKE * sqrt (mu / (a * a * a));
mm = (ee - ecc * sx) * R2D;
}
if (mm < 0.0)
mm += 360.0;
// Fill
orb.satno = 0;
orb.eqinc = incl * D2R;
orb.ascn = node * D2R;
orb.argp = peri * D2R;
orb.mnan = mm * D2R;
orb.ecc = ecc;
orb.rev = XKE * pow (a, -3.0 / 2.0) * XMNPDA / (2.0 * M_PI);
orb.bstar = 0.0;
orb.ep_year = ep_year;
orb.ep_day = ep_day;
orb.norb = 0;
return orb;
}
// State vector to SGP4 elements
orbit_t
rv2el (int satno, double mjd, xyz_t r0, xyz_t v0)
{
int i, imode;
orbit_t orb[5], orb1[5];
xyz_t r, v;
kep_t kep;
char line1[70], line2[70];
int ep_year;
double ep_day;
// Epoch
ep_day = mjd2doy (mjd, &ep_year);
// Initial guess
orb[0] = classel (ep_year, ep_day, r0, v0);
orb[0].satno = satno;
for (i = 0; i < 4; i++)
{
// Propagate
imode = init_sgdp4 (&orb[i]);
imode = satpos_xyz (mjd + 2400000.5, &r, &v);
// Compute initial orbital elements
orb1[i] = classel (ep_year, ep_day, r, v);
// Adjust
orb[i + 1].rev = orb[i].rev + orb[0].rev - orb1[i].rev;
orb[i + 1].ascn = orb[i].ascn + orb[0].ascn - orb1[i].ascn;
orb[i + 1].argp = orb[i].argp + orb[0].argp - orb1[i].argp;
orb[i + 1].mnan = orb[i].mnan + orb[0].mnan - orb1[i].mnan;
orb[i + 1].eqinc = orb[i].eqinc + orb[0].eqinc - orb1[i].eqinc;
orb[i + 1].ecc = orb[i].ecc + orb[0].ecc - orb1[i].ecc;
orb[i + 1].ep_year = orb[i].ep_year;
orb[i + 1].ep_day = orb[i].ep_day;
orb[i + 1].satno = orb[i].satno;
orb[i + 1].norb = orb[i].norb;
orb[i + 1].bstar = orb[i].bstar;
// Keep in range
if (orb[i + 1].ecc < 0.0)
orb[i + 1].ecc = 0.0;
if (orb[i + 1].eqinc < 0.0)
orb[i + 1].eqinc = 0.0;
}
orb[i].mnan = modulo (orb[i].mnan, 2.0 * M_PI);
orb[i].ascn = modulo (orb[i].ascn, 2.0 * M_PI);
orb[i].argp = modulo (orb[i].argp, 2.0 * M_PI);
return orb[i];
}
// Fit
void
fit (orbit_t orb, int *ia)
{
int i, n;
double a[7], da[7];
double db[7] = { 0.1, 0.1, 0.002, 0.1, 0.1, 0.01, 0.0001 };
// Copy parameters
a[0] = orb.eqinc * R2D;
da[0] = da[0] * R2D;
a[1] = orb.ascn * R2D;
da[1] = da[1] * R2D;
a[2] = orb.ecc;
a[3] = orb.argp * R2D;
da[3] = da[3] * R2D;
a[4] = orb.mnan * R2D;
da[4] = da[4] * R2D;
a[5] = orb.rev;
a[6] = orb.bstar;
for (i = 0; i < 7; i++)
{
if (ia[i] == 1)
da[i] = db[i];
else
da[i] = 0.0;
}
// Construct struct
// a[0]: inclination
// a[1]: RA of ascending node
// a[2]: eccentricity
// a[3]: argument of periastron
// a[4]: mean anomaly
// a[5]: revs per day
// a[6]: bstar
// Count highlighted points
for (i = 0, n = 0; i < d.n; i++)
if (d.p[i].flag == 1)
n++;
if (n > 0)
versafit (n, 7, a, da, chisq, 0.0, 1e-7, "n");
// Return parameters
orb.eqinc = RAD (a[0]);
orb.ascn = RAD (modulo (a[1], 360.0));
orb.ecc = a[2];
orb.argp = RAD (modulo (a[3], 360.0));
orb.mnan = RAD (modulo (a[4], 360.0));
orb.rev = a[5];
orb.bstar = a[6];
return;
}
int
main (int argc, char *argv[])
{
int i, j, k, arg = 0, satno = 0, satname = 0, usecatalog = 0, imode, m = 10;
long norb;
char *datafile, *catalog, filename[32];
int ia[7] = { 0, 0, 0, 0, 0, 0, 0 };
char line1[70], line2[70], desig[10];
double mjd, sma, perigee, apogee, xno;
float mag = 0.0, dm;
xyz_t r, v;
FILE *file;
// Decode options
while ((arg = getopt (argc, argv, "d:c:i:n:m:")) != -1)
{
switch (arg)
{
case 'd':
datafile = optarg;
break;
case 'c':
catalog = optarg;
usecatalog = 1;
break;
case 'i':
satno = atoi (optarg);
break;
case 'n':
norb = atoi (optarg);
if (norb < 0)
norb = 0;
break;
case 'm':
mag = atof (optarg);
break;
default:
return 0;
}
}
// Magnitude offset
dm = 5.0 * log10 (1000.0 / 40000.0);
// Reloop stderr
freopen ("/tmp/stderr.txt", "w", stderr);
// Decode filename
mjd = decode_filename (datafile, &satname);
// Read data
d = read_data (datafile, mjd);
// Write data
sprintf (filename, "%06d.xyz", satname);
file = fopen (filename, "w");
for (i = 0; i < d.n; i++)
fprintf (file, "%lf %f %f %f\n", d.p[i].mjd, d.p[i].r.x, d.p[i].r.y,
d.p[i].r.z);
fclose (file);
// Open elements
sprintf (filename, "%06d.tle", satname);
file = fopen (filename, "w");
// Estimate orbit
k = 504;
if (usecatalog == 0)
{
// Set initial state vector
r.x = d.p[k].r.x;
r.y = d.p[k].r.y;
r.z = d.p[k].r.z;
v.x = (d.p[k + 1].r.x - d.p[k].r.x) / 60.0;
v.y = (d.p[k + 1].r.y - d.p[k].r.y) / 60.0;
v.z = (d.p[k + 1].r.z - d.p[k].r.z) / 60.0;
// Estimate initial orbit from elements
orb = rv2el (99999, d.p[k].mjd, r, v);
strcpy (orb.desig, "14999A");
orb.norb = norb;
}
else
{
// Read orbit
orb = read_tle (catalog, satno);
strcpy (desig, orb.desig);
// Propagate
imode = init_sgdp4 (&orb);
imode = satpos_xyz (d.p[k].mjd + 2400000.5, &r, &v);
orb = rv2el (orb.satno, d.p[k].mjd, r, v);
// orb.satno=99999;
// strcpy(orb.desig,"14999A");
strcpy (orb.desig, desig);
orb.norb = norb;
}
// Set flags
for (j = 0; j < d.n; j++)
d.p[j].flag = 0;
for (j = 0; j < d.n; j++)
d.p[j].flag = 1;
// Fit orbit
for (j = 0; j < 10; j++)
{
if (j == 1)
ia[4] = 1;
if (j == 2)
ia[1] = 1;
if (j == 3)
ia[0] = 1;
if (j == 4)
ia[5] = 1;
if (j == 5)
ia[3] = 1;
if (j == 6)
ia[2] = 1;
if (j == 7)
ia[6] = 1;
fit (orb, ia);
}
// Compute orbit size
xno = orb.rev * 2.0 * M_PI / XMNPDA;
sma = pow (XKE / xno, 2.0 / 3.0) * XKMPER;
perigee = sma * (1.0 - orb.ecc) - XKMPER;
apogee = sma * (1.0 + orb.ecc) - XKMPER;
// Format TLE
format_tle (orb, line1, line2);
fprintf (file,
"SO %6d %4.1f %7.0fkm x%7.0fkm\n%s\n%s\n# %d positions, %.1f km rms\n",
satname, mag + dm, perigee, apogee, line1, line2, d.nsel, d.rms);
printf
("SO %6d %4.1f %7.0fkm x%7.0fkm\n%s\n%s\n# %d positions, %.1f km rms\n",
satname, mag + dm, perigee, apogee, line1, line2, d.nsel, d.rms);
// Close output file
fclose (file);
return 0;
}
View Git Blame Copy Permalink