static void Main(string[] args)
{
// string line0 = "ISS (ZARYA) ";
string line1 = "1 25544U 98067A 20206.38292522 -.00000985 00000-0 -95291-5 0 9998";
string line2 = "2 25544 51.6430 164.3636 0001088 140.8410 323.1994 15.49511774237787";
Sat_Io io = new Sat_Io();
Satrec satrec = io.twoline2satrec(line1, line2);
Sgp4 sgp4 = new Sgp4();
PositionAndVelocity positionAndVelocity = sgp4.sgp4(satrec, 0);
// Set the Observer at 122.03 West by 36.96 North, in RADIANS
Geodetic observerGd = new Geodetic();
Transform tf = new Transform();
observerGd.longitude = tf.degreesToRadians(-122.0308);
observerGd.latitude = tf.degreesToRadians(36.9613422);
observerGd.height = 0.370;
Console.WriteLine(positionAndVelocity.position_ECI.x);
Console.WriteLine(positionAndVelocity.position_ECI.y);
Console.WriteLine(positionAndVelocity.position_ECI.z);
Console.WriteLine("Look i didnt crash");
}
public void CheckInitialisation()
{
string line1 = "1 25544U 98067A 20206.38292522 -.00000985 00000-0 -95291-5 0 9998";
string line2 = "2 25544 51.6430 164.3636 0001088 140.8410 323.1994 15.49511774237787";
Sat.Sat_Io io = new Sat.Sat_Io();
Sat.Satrec satrec = io.twoline2satrec(line1, line2);
Sat.Sgp4 sgp4 = new Sat.Sgp4();
Sat.PositionAndVelocity positionAndVelocity = sgp4.sgp4(satrec, 0);
Assert.Equal(485.6716711104389, positionAndVelocity.position_ECI.x); // Value from Sat.js
Assert.Equal(-4381.651985814846, positionAndVelocity.position_ECI.y); // Value from Sat.js
Assert.Equal(5162.784591395867, positionAndVelocity.position_ECI.z); // Value from Sat.js
}
/*----------------------------------------------------------------------------
*
* procedure sgp4
*
* this procedure is the sgp4 prediction model from space command. this is an
* updated and combined version of sgp4 and sdp4, which were originally
* published separately in spacetrack report //3. this version follows the
* methodology from the aiaa paper (2006) describing the history and
* development of the code.
*
* author : david vallado 719-573-2600 28 jun 2005
*
* inputs :
* satrec - initialised structure from sgp4init() call.
* tsince - time since epoch (minutes)
*
* outputs :
* r - position vector km
* v - velocity km/sec
* return code - non-zero on error.
* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er
* 2 - mean motion less than 0.0
* 3 - pert elements, ecc < 0.0 or ecc > 1.0
* 4 - semi-latus rectum < 0.0
* 5 - epoch elements are sub-orbital
* 6 - satellite has decayed
*
* locals :
* am -
* axnl, aynl -
* betal -
* cosim , sinim , cosomm , sinomm , cnod , snod , cos2u ,
* sin2u , coseo1 , sineo1 , cosi , sini , cosip , sinip ,
* cosisq , cossu , sinsu , cosu , sinu
* delm -
* delomg -
* dndt -
* eccm -
* emsq -
* ecose -
* el2 -
* eo1 -
* eccp -
* esine -
* argpm -
* argpp -
* omgadf -
* pl -
* r -
* rtemsq -
* rdotl -
* rl -
* rvdot -
* rvdotl -
* su -
* t2 , t3 , t4 , tc
* tem5, temp , temp1 , temp2 , tempa , tempe , templ
* u , ux , uy , uz , vx , vy , vz
* inclm - inclination
* mm - mean anomaly
* nm - mean motion
* nodem - right asc of ascending node
* xinc -
* xincp -
* xl -
* xlm -
* mp -
* xmdf -
* xmx -
* xmy -
* nodedf -
* xnode -
* nodep -
* np -
*
* coupling :
* getgravconst-
* dpper
* dspace
*
* references :
* hoots, roehrich, norad spacetrack report //3 1980
* hoots, norad spacetrack report //6 1986
* hoots, schumacher and glover 2004
* vallado, crawford, hujsak, kelso 2006
* ----------------------------------------------------------------------------*/
public PositionAndVelocity sgp4(Satrec satrec, double tsince)
{
Globals globals = new Globals();
double pi = globals.pi;
double twoPi = globals.twoPi;
double earthRadius = globals.earthRadius;
double xke = globals.xke;
double vkmpersec = globals.vkmpersec;
double j2 = globals.j2;
double j3oj2 = globals.j3oj2;
double x2o3 = globals.x2o3;
double coseo1 = 0.0;
double sineo1 = 0.0;
double cosip = 0.0;
double sinip = 0.0;
double cosisq = 0.0;
double delm = 0.0;
double delomg = 0.0;
double eo1 = 0.0;
double argpm = 0.0;
double argpp = 0.0;
double su = 0.0;
double t3 = 0.0;
double t4 = 0.0;
double tc = 0.0;
double tem5 = 0.0;
double temp = 0.0;
double tempa = 0.0;
double tempe = 0.0;
double templ = 0.0;
double inclm = 0.0;
double mm = 0.0;
double nm = 0.0;
double nodem = 0.0;
double xincp = 0.0;
double xlm = 0.0;
double mp = 0.0;
double nodep = 0.0;
/* ------------------ set mathematical constants --------------- */
// sgp4fix divisor for divide by zero check on inclination
// the old check used 1.0 + cos(pi-1.0e-9), but then compared it to
// 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency
double temp4 = 1.5e-12;
// --------------------- clear sgp4 error flag -----------------
satrec.t = tsince;
satrec.error = 0;
// ------- update for secular gravity and atmospheric drag -----
double xmdf = satrec.mo + (satrec.mdot * satrec.t);
double argpdf = satrec.argpo + (satrec.argpdot * satrec.t);
double nodedf = satrec.nodeo + (satrec.nodedot * satrec.t);
argpm = argpdf;
mm = xmdf;
double t2 = satrec.t * satrec.t;
nodem = nodedf + (satrec.nodecf * t2);
tempa = 1.0 - (satrec.cc1 * satrec.t);
tempe = satrec.bstar * satrec.cc4 * satrec.t;
templ = satrec.t2cof * t2;
if (satrec.isimp != 1)
{
delomg = satrec.omgcof * satrec.t;
// sgp4fix use mutliply for speed instead of pow
double delmtemp = 1.0 + (satrec.eta * Math.Cos(xmdf));
delm = satrec.xmcof * ((delmtemp * delmtemp * delmtemp) - satrec.delmo);
temp = delomg + delm;
mm = xmdf + temp;
argpm = argpdf - temp;
t3 = t2 * satrec.t;
t4 = t3 * satrec.t;
tempa = tempa - (satrec.d2 * t2) - (satrec.d3 * t3) - (satrec.d4 * t4);
tempe += satrec.bstar * satrec.cc5 * (Math.Sin(mm) - satrec.sinmao);
templ = templ + (satrec.t3cof * t3) + (t4 * (satrec.t4cof + (satrec.t * satrec.t5cof)));
}
nm = satrec.no;
double em = satrec.ecco;
inclm = satrec.inclo;
if (satrec.method == 'd')
{
tc = satrec.t;
DspaceOptions dspaceOptions = new DspaceOptions();
dspaceOptions.irez = satrec.irez;
dspaceOptions.d2201 = satrec.d2201;
dspaceOptions.d2211 = satrec.d2211;
dspaceOptions.d3210 = satrec.d3210;
dspaceOptions.d3222 = satrec.d3222;
dspaceOptions.d4410 = satrec.d4410;
dspaceOptions.d4422 = satrec.d4422;
dspaceOptions.d5220 = satrec.d5220;
dspaceOptions.d5232 = satrec.d5232;
dspaceOptions.d5421 = satrec.d5421;
dspaceOptions.d5433 = satrec.d5433;
dspaceOptions.dedt = satrec.dedt;
dspaceOptions.del1 = satrec.del1;
dspaceOptions.del2 = satrec.del2;
dspaceOptions.del3 = satrec.del3;
dspaceOptions.didt = satrec.didt;
dspaceOptions.dmdt = satrec.dmdt;
dspaceOptions.dnodt = satrec.dnodt;
dspaceOptions.domdt = satrec.domdt;
dspaceOptions.argpo = satrec.argpo;
dspaceOptions.argpdot = satrec.argpdot;
dspaceOptions.t = satrec.t;
dspaceOptions.tc = tc;
dspaceOptions.gsto = satrec.gsto;
dspaceOptions.xfact = satrec.xfact;
dspaceOptions.xlamo = satrec.xlamo;
dspaceOptions.no = satrec.no;
dspaceOptions.atime = satrec.atime;
dspaceOptions.em = em;
dspaceOptions.argpm = argpm;
dspaceOptions.inclm = inclm;
dspaceOptions.xli = satrec.xli;
dspaceOptions.mm = mm;
dspaceOptions.xni = satrec.xni;
dspaceOptions.nodem = nodem;
dspaceOptions.nm = nm;
Dspace dspace = new Dspace();
DspaceResults dspaceResults = new DspaceResults();
dspaceResults = dspace.dspace(dspaceOptions);
em = dspaceResults.em;
argpm = dspaceResults.argpm;
inclm = dspaceResults.inclm;
mm = dspaceResults.mm;
nodem = dspaceResults.nodem;
nm = dspaceResults.nm;
}
if (nm <= 0.0)
{
// printf("// error nm %f\n", nm);
satrec.error = 2;
// sgp4fix add return
PositionAndVelocity noValue = new PositionAndVelocity();
return(noValue); //TODO set this to 0
}
double am = (Math.Pow((xke / nm), x2o3)) * tempa * tempa;
nm = xke / (Math.Pow(am, 1.5));
em -= tempe;
// fix tolerance for error recognition
// sgp4fix am is fixed from the previous nm check
if (em >= 1.0 || em < -0.001) // || (am < 0.95)
// printf("// error em %f\n", em);
{
satrec.error = 1;
PositionAndVelocity noValue = new PositionAndVelocity();
return(noValue); //TODO set this to 0
}
// sgp4fix fix tolerance to avoid a divide by zero
if (em < 1.0e-6)
{
em = 1.0e-6;
}
mm += satrec.no * templ;
xlm = mm + argpm + nodem;
nodem %= twoPi;
argpm %= twoPi;
xlm %= twoPi;
mm = (xlm - argpm - nodem) % twoPi;
// ----------------- compute extra mean quantities -------------
double sinim = Math.Sin(inclm);
double cosim = Math.Cos(inclm);
// -------------------- add lunar-solar periodics --------------
double ep = em;
xincp = inclm;
argpp = argpm;
nodep = nodem;
mp = mm;
sinip = sinim;
cosip = cosim;
if (satrec.method == 'd')
{
DpperOptions dpperParameters = new DpperOptions();
dpperParameters.inclo = satrec.inclo;
dpperParameters.init = 'n';
dpperParameters.ep = ep;
dpperParameters.inclp = xincp;
dpperParameters.nodep = nodep;
dpperParameters.argpp = argpp;
dpperParameters.mp = mp;
dpperParameters.opsmode = satrec.operationmode;
Dpper dpper = new Dpper();
DpperResult dpperResult = new DpperResult();
dpperResult = dpper.dpper(satrec, dpperParameters);
ep = dpperResult.ep;
nodep = dpperResult.nodep;
argpp = dpperResult.argpp;
mp = dpperResult.mp;
xincp = dpperResult.inclp;
if (xincp < 0.0)
{
xincp = -xincp;
nodep += pi;
argpp -= pi;
}
if (ep < 0.0 || ep > 1.0)
{
// printf("// error ep %f\n", ep);
satrec.error = 3;
// sgp4fix add return
PositionAndVelocity noValue = new PositionAndVelocity();
return(noValue); //TODO set this to 0
}
}
// -------------------- long period periodics ------------------
if (satrec.method == 'd')
{
sinip = Math.Sin(xincp);
cosip = Math.Cos(xincp);
satrec.aycof = -0.5 * j3oj2 * sinip;
// sgp4fix for divide by zero for xincp = 180 deg
if (Math.Abs(cosip + 1.0) > 1.5e-12)
{
satrec.xlcof = (-0.25 * j3oj2 * sinip * (3.0 + (5.0 * cosip))) / (1.0 + cosip);
}
else
{
satrec.xlcof = (-0.25 * j3oj2 * sinip * (3.0 + (5.0 * cosip))) / temp4;
}
}
double axnl = ep * Math.Cos(argpp);
temp = 1.0 / (am * (1.0 - (ep * ep)));
double aynl = (ep * Math.Sin(argpp)) + (temp * satrec.aycof);
double xl = mp + argpp + nodep + (temp * satrec.xlcof * axnl);
// --------------------- solve kepler's equation ---------------
double u = (xl - nodep) % twoPi;
eo1 = u;
tem5 = 9999.9;
double ktr = 1;
// sgp4fix for kepler iteration
// the following iteration needs better limits on corrections
while (Math.Abs(tem5) >= 1.0e-12 && ktr <= 10)
{
sineo1 = Math.Sin(eo1);
coseo1 = Math.Cos(eo1);
tem5 = 1.0 - (coseo1 * axnl) - (sineo1 * aynl);
tem5 = (((u - (aynl * coseo1)) + (axnl * sineo1)) - eo1) / tem5;
if (Math.Abs(tem5) >= 0.95)
{
if (tem5 > 0.0)
{
tem5 = 0.95;
}
else
{
tem5 = -0.95;
}
}
eo1 += tem5;
ktr += 1;
}
// ------------- short period preliminary quantities -----------
double ecose = (axnl * coseo1) + (aynl * sineo1);
double esine = (axnl * sineo1) - (aynl * coseo1);
double el2 = (axnl * axnl) + (aynl * aynl);
double pl = am * (1.0 - el2);
if (pl < 0.0)
{
// printf("// error pl %f\n", pl);
satrec.error = 4;
// sgp4fix add return
PositionAndVelocity noValue = new PositionAndVelocity();
return(noValue); //TODO set this to 0
}
double rl = am * (1.0 - ecose);
double rdotl = (Math.Sqrt(am) * esine) / rl;
double rvdotl = Math.Sqrt(pl) / rl;
double betal = Math.Sqrt(1.0 - el2);
temp = esine / (1.0 + betal);
double sinu = (am / rl) * (sineo1 - aynl - (axnl * temp));
double cosu = (am / rl) * ((coseo1 - axnl) + (aynl * temp));
su = Math.Atan2(sinu, cosu);
double sin2u = (cosu + cosu) * sinu;
double cos2u = 1.0 - (2.0 * sinu * sinu);
temp = 1.0 / pl;
double temp1 = 0.5 * j2 * temp;
double temp2 = temp1 * temp;
// -------------- update for short period periodics ------------
if (satrec.method == 'd')
{
cosisq = cosip * cosip;
satrec.con41 = (3.0 * cosisq) - 1.0;
satrec.x1mth2 = 1.0 - cosisq;
satrec.x7thm1 = (7.0 * cosisq) - 1.0;
}
double mrt = (rl * (1.0 - (1.5 * temp2 * betal * satrec.con41)))
+ (0.5 * temp1 * satrec.x1mth2 * cos2u);
// sgp4fix for decaying satellites
if (mrt < 1.0)
{
// printf("// decay condition %11.6f \n",mrt);
satrec.error = 6;
// return {
// position: false,
// velocity: false,
// };
PositionAndVelocity noValue = new PositionAndVelocity();
return(noValue); //TODO set this to 0
}
su -= 0.25 * temp2 * satrec.x7thm1 * sin2u;
double xnode = nodep + (1.5 * temp2 * cosip * sin2u);
double xinc = xincp + (1.5 * temp2 * cosip * sinip * cos2u);
double mvt = rdotl - ((nm * temp1 * satrec.x1mth2 * sin2u) / xke);
double rvdot = rvdotl + ((nm * temp1 * ((satrec.x1mth2 * cos2u) + (1.5 * satrec.con41))) / xke);
// --------------------- orientation vectors -------------------
double sinsu = Math.Sin(su);
double cossu = Math.Cos(su);
double snod = Math.Sin(xnode);
double cnod = Math.Cos(xnode);
double sini = Math.Sin(xinc);
double cosi = Math.Cos(xinc);
double xmx = -snod * cosi;
double xmy = cnod * cosi;
double ux = (xmx * sinsu) + (cnod * cossu);
double uy = (xmy * sinsu) + (snod * cossu);
double uz = sini * sinsu;
double vx = (xmx * cossu) - (cnod * sinsu);
double vy = (xmy * cossu) - (snod * sinsu);
double vz = sini * cossu;
// --------- position and velocity (in km and km/sec) ----------
PositionAndVelocity positionAndVelocity = new PositionAndVelocity();
positionAndVelocity.position_ECI.x = (mrt * ux) * earthRadius;
positionAndVelocity.position_ECI.y = (mrt * uy) * earthRadius;
positionAndVelocity.position_ECI.z = (mrt * uz) * earthRadius;
// double r = {
// x: (mrt * ux) * earthRadius,
// y: (mrt * uy) * earthRadius,
// z: (mrt * uz) * earthRadius,
// };
positionAndVelocity.velocity_ECI.x = ((mvt * ux) + (rvdot * vx)) * vkmpersec;
positionAndVelocity.velocity_ECI.y = ((mvt * uy) + (rvdot * vy)) * vkmpersec;
positionAndVelocity.velocity_ECI.z = ((mvt * uz) + (rvdot * vz)) * vkmpersec;
// double v = {
// x: ((mvt * ux) + (rvdot * vx)) * vkmpersec,
// y: ((mvt * uy) + (rvdot * vy)) * vkmpersec,
// z: ((mvt * uz) + (rvdot * vz)) * vkmpersec,
// };
// return {
// position: r,
// velocity: v,
// };
return(positionAndVelocity);
}
