// 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];
}
/**
* Update the global variables p_eci, v_eci with SGP4 propagator
* @return The state of SGP4 propagator
*/
adcs_error_status update_sgp4() {
sgp4_status satpos_status;
/* Update position and velocity of satellite */
satpos_status = satpos_xyz(adcs_time.jd, &p_eci, &v_eci);
/* Check if the SGP4 is correct */
if (satpos_status == SGP4_ZERO_ECC || satpos_status == SGP4_NEAR_SIMP
|| satpos_status == SGP4_NEAR_NORM) {
return ERROR_OK;
}
return ERROR_SGP4;
}
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;
}
// 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;
}
int main(int argc, char *argv[])
{
orbit_t orb;
xyz_t pos, vel;
double jd, tsince;
long satno=0;
int imode;
char filename[ST_SIZE] = "twoline.txt";
FILE *fp=NULL;
char ctrl_name[ST_SIZE] = "ssd.txt";
long ii,imax=0,iend=0;
/* Data for the prediction type and time period */
double ts = 0.0, delta=0.0; /* Time since TLE epoch to start predictions */
int bad=1;
int c;
time_t tnow;
time(&tnow);
fprintf(stderr, "# Start time = %s", ctime(&tnow));
GETOPT(c, options)
{
case 'c': strncpy(ctrl_name, optarg, ST_SIZE-1); break;
case 'i': strncpy(filename, optarg, ST_SIZE-1); break;
case 's': satno = atol(optarg); break;
case 'v': Verbose = atoi(optarg); break;
default:
fprintf(stderr, usage, argv[0]);
exit(1);
break;
}
if((fp=fopen(ctrl_name, "rb")) != NULL)
{
if(fscanf(fp, " %lf %lf %ld %ld %ld", &ts, &delta, &imax, &iend, &satno) == 5)
{
bad=0;
}
fclose(fp);
}
if(bad)
{
fprintf(stderr, "Failed to read ssd.txt file for 'tstart,delta,imax,isat'\n");
return 1;
}
fp = fopen(filename, "rb");
if(fp == NULL)
{
fatal_error("File open failed for reading \"%s\"", filename);
}
while(read_twoline(fp, satno, &orb) == 0)
{
print_orb(&orb);
Isat = orb.satno;
imode = init_sgdp4(&orb);
switch(imode)
{
case SGDP4_ERROR : printf("# SGDP error\n"); break;
case SGDP4_NOT_INIT : printf("# SGDP not init\n"); break;
case SGDP4_ZERO_ECC : printf("# SGDP zero ecc\n"); break;
case SGDP4_NEAR_SIMP : printf("# SGP4 simple\n"); break;
case SGDP4_NEAR_NORM : printf("# SGP4 normal\n"); break;
case SGDP4_DEEP_NORM : printf("# SDP4 normal\n"); break;
case SGDP4_DEEP_RESN : printf("# SDP4 resonant\n"); break;
case SGDP4_DEEP_SYNC : printf("# SDP4 synchronous\n"); break;
default: printf("# SGDP mode not recognised!\n");
}
if(imode == SGDP4_ERROR) continue;
printf("# Satellite number\n");
printf("%05ld\n", (long)orb.satno);
printf("# Tsince X Y Z Xdot Ydot Zdot\n");
/* Run test like the original SGP doccument */
for(ii=0; ii <= imax; ii++)
{
if(ii != iend)
{
tsince=ts + ii*delta;
}
else
{
tsince=ts + delta;
}
jd = SGDP4_jd0 + tsince / 1440.0;
if(satpos_xyz(jd, &pos, &vel) == SGDP4_ERROR) break;
printf("%12.4f %16.8f %16.8f %16.8f %16.12f %16.12f %16.12f\n",
tsince,
pos.x, pos.y, pos.z,
vel.x, vel.y, vel.z);
}
}
time(&tnow);
fprintf(stderr, "# End time = %s", ctime(&tnow));
exit(0);
return 0;
}
int main(int argc,char *argv[])
{
int imode,satno=0,arg,usecatalog=0,satnomin,flag=0;
FILE *file;
orbit_t orb;
xyz_t r,v,n,m,nm,r0,v0,mr,mv;
char tlefile[LIM],catalog[LIM],nfd[32];
char line1[80],line2[80],desig[20];
double mjd,ra,de,dr,drmin,dmr,dmv;
char *env;
// Get environment variable
env=getenv("ST_TLEDIR");
sprintf(tlefile,"%s/classfd.tle",env);
// Set date
nfd_now(nfd);
mjd=nfd2mjd(nfd);
// Decode options
while ((arg=getopt(argc,argv,"C:c:i:t:m:h"))!=-1) {
switch (arg) {
case 't':
strcpy(nfd,optarg);
mjd=nfd2mjd(nfd);
break;
case 'm':
mjd=(double) atof(optarg);
break;
case 'c':
strcpy(tlefile,optarg);
break;
case 'C':
strcpy(catalog,optarg);
usecatalog=1;
break;
case 'i':
satno=atoi(optarg);
break;
case 'h':
usage();
return 0;
break;
default:
usage();
return 0;
}
}
// Reloop stderr
freopen("/tmp/stderr.txt","w",stderr);
// Open file
file=fopen(tlefile,"r");
while (read_twoline(file,satno,&orb)==0) {
format_tle(orb,line1,line2);
// Propagate
imode=init_sgdp4(&orb);
imode=satpos_xyz(mjd+2400000.5,&r0,&v0);
// Compute normal
n=cross(r0,v0);
ra=modulo(atan2(n.y,n.x)*R2D,360.0);
de=asin(n.z/magnitude(n))*R2D;
if (usecatalog==0)
printf("%05d %14.8lf %10.6f %10.6f %10.2f %10.2f %10.2f %10.2f\n",orb.satno,mjd,ra,de,magnitude(n),n.x,n.y,n.z);
}
fclose(file);
// Match against a catalog
if (usecatalog==1) {
// Open file
file=fopen(catalog,"r");
while (read_twoline(file,0,&orb)==0) {
format_tle(orb,line1,line2);
// Propagate
imode=init_sgdp4(&orb);
imode=satpos_xyz(mjd+2400000.5,&r,&v);
// Compute normal
m=cross(r,v);
// Normal difference
nm.x=n.x-m.x;
nm.y=n.y-m.y;
nm.z=n.z-m.z;
dr=magnitude(nm);
// Position/velocity difference
mr.x=r0.x-r.x;
mr.y=r0.y-r.y;
mr.z=r0.z-r.z;
mv.x=v0.x-v.x;
mv.y=v0.y-v.y;
mv.z=v0.z-v.z;
if (flag==0 || dr<drmin) {
drmin=dr;
dmr=magnitude(mr);
dmv=magnitude(mv);
satnomin=orb.satno;
flag=1;
}
}
printf("%05d %05d %8.2f km %8.2f km %10.6f km/s\n",satno,satnomin,drmin,dmr,dmv);
}
return 0;
}
int main(int argc,char *argv[])
{
int imode,arg,satno=0,useepoch=0,format=0,gmat=0;
FILE *file;
char *env;
char tlefile[LIM],nfd[32];
double mjd;
xyz_t r,v;
orbit_t orb;
double rr[3],vv[3],e[3][3],et[3][3];
// Get environment variable
env=getenv("ST_TLEDIR");
sprintf(tlefile,"%s/classfd.tle",env);
// Set date
nfd_now(nfd);
mjd=nfd2mjd(nfd);
// Decode options
while ((arg=getopt(argc,argv,"c:i:t:m:hejg"))!=-1) {
switch (arg) {
case 't':
strcpy(nfd,optarg);
mjd=nfd2mjd(nfd);
break;
case 'm':
mjd=atof(optarg);
break;
case 'e':
useepoch=1;
break;
case 'j':
format=1;
break;
case 'g':
gmat=1;
break;
case 'c':
strcpy(tlefile,optarg);
break;
case 'i':
satno=atoi(optarg);
break;
case 'h':
usage();
return 0;
break;
default:
usage();
return 0;
}
}
// Open file
file=fopen(tlefile,"r");
while (read_twoline(file,satno,&orb)==0) {
// Propagate
imode=init_sgdp4(&orb);
// Use epoch instead of user supplied date
if (useepoch==1)
mjd=SGDP4_jd0-2400000.5;
// Compute position and velocity
imode=satpos_xyz(mjd+2400000.5,&r,&v);
// Output
if (format==0 && gmat==0)
printf("%05d %14.8lf %f %f %f %f %f %f TEME\n",orb.satno,mjd,r.x,r.y,r.z,v.x,v.y,v.z);
// To vectors
rr[0]=r.x;
rr[1]=r.y;
rr[2]=r.z;
vv[0]=v.x;
vv[1]=v.y;
vv[2]=v.z;
// Matrices
icrs_to_teme(mjd,e);
matrix_transpose(e,et);
// Transform
vector_multiply_in_place(et,rr);
vector_multiply_in_place(et,vv);
// Output J2000
if (format==1 && gmat==0)
printf("%05d %14.8lf %f %f %f %f %f %f J2000\n",orb.satno,mjd,rr[0],rr[1],rr[2],vv[0],vv[1],vv[2]);
// GMAT output
if (gmat==1) {
printf("UTCModJulian = %14.8lf\n",mjd-29999.5);
printf("CoordinateSystem = EarthMJ2000Eq\n");
printf("X = %lf\n",rr[0]);
printf("Y = %lf\n",rr[1]);
printf("Z = %lf\n",rr[2]);
printf("VX = %lf\n",vv[0]);
printf("VY = %lf\n",vv[1]);
printf("VZ = %lf\n",vv[2]);
}
}
fclose(file);
return 0;
}
int main(int argc,char *argv[])
{
int imode,satno=0,arg,desigflag=0;
FILE *file;
orbit_t orb;
xyz_t r,v;
char tlefile[LIM],nfd[32];
char line1[80],line2[80],desig[20];
double mjd,epoch,ep_day,bstar;
char *env;
int ep_year;
// Get environment variable
env=getenv("ST_TLEDIR");
sprintf(tlefile,"%s/classfd.tle",env);
// Set date
nfd_now(nfd);
mjd=nfd2mjd(nfd);
// Decode options
while ((arg=getopt(argc,argv,"c:i:t:m:he:d:"))!=-1) {
switch (arg) {
case 't':
strcpy(nfd,optarg);
mjd=nfd2mjd(nfd);
break;
case 'e':
epoch=(double) atof(optarg);
ep_year=(int) floor(epoch/1000.0);
ep_day=epoch-1000*ep_year;
printf("%d %f\n",ep_year,ep_day);
if (ep_year<50)
ep_year+=2000;
else
ep_year+=1900;
mjd=doy2mjd(ep_year,ep_day);
break;
case 'd':
strcpy(desig,optarg);
desigflag=1;
break;
case 'm':
mjd=(double) atof(optarg);
break;
case 'c':
strcpy(tlefile,optarg);
break;
case 'i':
satno=atoi(optarg);
break;
case 'h':
usage();
return 0;
break;
default:
usage();
return 0;
}
}
// Open file
file=fopen(tlefile,"r");
while (read_twoline(file,satno,&orb)==0) {
format_tle(orb,line1,line2);
// printf("Input:\n%s\n%s\n",line1,line2);
if (desigflag==0)
strcpy(desig,orb.desig);
bstar=orb.bstar;
// Propagate
imode=init_sgdp4(&orb);
imode=satpos_xyz(mjd+2400000.5,&r,&v);
// Convert
orb=rv2el(orb.satno,mjd,r,v);
// Copy back
strcpy(orb.desig,desig);
orb.bstar=bstar;
format_tle(orb,line1,line2);
printf("%s\n%s\n",line1,line2);
}
fclose(file);
return 0;
}
