/* oblate spheroid as defined in WGS '72. */
void
Calculate_LatLonAlt(double _time, vector_t *pos, geodetic_t *geodetic)
{
/* Reference: The 1992 Astronomical Almanac, page K12. */
double r,e2,phi,c;
geodetic->theta = AcTan(pos->y,pos->x);/*radians*/
geodetic->lon = FMod2p(geodetic->theta - ThetaG_JD(_time));/*radians*/
r = sqrt(Sqr(pos->x) + Sqr(pos->y));
e2 = __f*(2 - __f);
geodetic->lat = AcTan(pos->z,r);/*radians*/
do
{
phi = geodetic->lat;
c = 1/sqrt(1 - e2*Sqr(sin(phi)));
geodetic->lat = AcTan(pos->z + xkmper*c*e2*sin(phi),r);
}
while(fabs(geodetic->lat - phi) >= 1E-10);
geodetic->alt = r/cos(geodetic->lat) - xkmper*c;/*kilometers*/
if( geodetic->lat > pio2 ) geodetic->lat -= twopi;
} /*Procedure Calculate_LatLonAlt*/
void Calculate_LatLonAlt(uint64_t time, PredicThirteen::vector_t *pos, PredicThirteen::geodetic_t *geodetic)
{
/* Procedure Calculate_LatLonAlt will calculate the geodetic */
/* position of an object given its ECI position pos and time. */
/* It is intended to be used to determine the ground track of */
/* a satellite. The calculations assume the earth to be an */
/* oblate spheroid as defined in WGS '72. */
/* Reference: The 1992 Astronomical Almanac, page K12. */
float r, e2, phi, c;
geodetic->theta=AcTan(pos->y,pos->x); /* radians */
geodetic->lon=FMod2p(geodetic->theta-ThetaG_JD(time)); /* radians */
r=sqrt(Sqr(pos->x)+Sqr(pos->y));
e2=f*(2-f);
geodetic->lat=AcTan(pos->z,r); /* radians */
do
{
phi=geodetic->lat;
c=1/sqrt(1-e2*Sqr(sin(phi)));
geodetic->lat=AcTan(pos->z+xkmper*c*e2*sin(phi),r);
} while (fabs(geodetic->lat-phi)>=1E-10);
geodetic->alt=r/cos(geodetic->lat)-xkmper*c; /* kilometers */
if (geodetic->lat>pio2)
geodetic->lat-=twopi;
}
gVector gSatTEME::computeSubPoint(gTime ai_Time)
{
gVector resultVector(3); // (0) Latitude, (1) Longitude, (2) altitude
double theta, r, e2, phi, c;
theta = AcTan(m_Position[1], m_Position[0]); // radians
resultVector[ LONGITUDE] = fmod((theta - ai_Time.toThetaGMST()), K2PI); //radians
r = std::sqrt(Sqr(m_Position[0]) + Sqr(m_Position[1]));
e2 = __f*(2 - __f);
resultVector[ LATITUDE] = AcTan(m_Position[2],r); /*radians*/
do
{
phi = resultVector[ LATITUDE];
c = 1/std::sqrt(1 - e2*Sqr(sin(phi)));
resultVector[ LATITUDE] = AcTan(m_Position[2] + KEARTHRADIUS*c*e2*sin(phi),r);
}
while(fabs(resultVector[ LATITUDE] - phi) >= 1E-10);
resultVector[ ALTITUDE] = r/cos(resultVector[ LATITUDE]) - KEARTHRADIUS*c;/*kilometers*/
if(resultVector[ LATITUDE] > (KPI/2.0)) resultVector[ LATITUDE] -= K2PI;
resultVector[LATITUDE] = resultVector[LATITUDE]/KDEG2RAD;
resultVector[LONGITUDE] = resultVector[LONGITUDE]/KDEG2RAD;
if(resultVector[LONGITUDE] < -180.0) resultVector[LONGITUDE] +=360;
else if(resultVector[LONGITUDE] > 180.0) resultVector[LONGITUDE] -= 360;
return resultVector;
}
void Calculate_LatLonAlt(double time, const double pos[3], geodetic_t *geodetic)
{
/* Procedure Calculate_LatLonAlt will calculate the geodetic */
/* position of an object given its ECI position pos and time. */
/* It is intended to be used to determine the ground track of */
/* a satellite. The calculations assume the earth to be an */
/* oblate spheroid as defined in WGS '72. */
/* Reference: The 1992 Astronomical Almanac, page K12. */
double r, e2, phi, c;
//Convert to julian time:
time += 2444238.5;
geodetic->theta = AcTan(pos[1], pos[0]); /* radians */
geodetic->lon = FMod2p(geodetic->theta-ThetaG_JD(time)); /* radians */
r = sqrt(Sqr(pos[0])+Sqr(pos[1]));
e2 = f*(2-f);
geodetic->lat=AcTan(pos[2],r); /* radians */
do
{
phi=geodetic->lat;
c=1/sqrt(1-e2*Sqr(sin(phi)));
geodetic->lat=AcTan(pos[2]+xkmper*c*e2*sin(phi),r);
} while (fabs(geodetic->lat-phi)>=1E-10);
geodetic->alt=r/cos(geodetic->lat)-xkmper*c; /* kilometers */
if (geodetic->lat>pio2)
geodetic->lat-= 2*M_PI;
}
void
Calculate_RADec_and_Obs ( double _time,
vector_t *pos,
vector_t *vel,
geodetic_t *geodetic,
obs_astro_t *obs_set)
{
/* Reference: Methods of Orbit Determination by */
/* Pedro Ramon Escobal, pp. 401-402 */
double phi,theta,sin_theta,cos_theta,sin_phi,cos_phi,
az,el,Lxh,Lyh,Lzh,Sx,Ex,Zx,Sy,Ey,Zy,Sz,Ez,Zz,
Lx,Ly,Lz,cos_delta,sin_alpha,cos_alpha;
obs_set_t obs;
Calculate_Obs(_time,pos,vel,geodetic,&obs);
/* if( isFlagSet(VISIBLE_FLAG) )
{*/
az = obs.az;
el = obs.el;
phi = geodetic->lat;
theta = FMod2p(ThetaG_JD(_time) + geodetic->lon);
sin_theta = sin(theta);
cos_theta = cos(theta);
sin_phi = sin(phi);
cos_phi = cos(phi);
Lxh = -cos(az) * cos(el);
Lyh = sin(az) * cos(el);
Lzh = sin(el);
Sx = sin_phi * cos_theta;
Ex = -sin_theta;
Zx = cos_theta * cos_phi;
Sy = sin_phi * sin_theta;
Ey = cos_theta;
Zy = sin_theta*cos_phi;
Sz = -cos_phi;
Ez = 0;
Zz = sin_phi;
Lx = Sx*Lxh + Ex * Lyh + Zx*Lzh;
Ly = Sy*Lxh + Ey * Lyh + Zy*Lzh;
Lz = Sz*Lxh + Ez * Lyh + Zz*Lzh;
obs_set->dec = ArcSin(Lz); /* Declination (radians)*/
cos_delta = sqrt(1 - Sqr(Lz));
sin_alpha = Ly / cos_delta;
cos_alpha = Lx / cos_delta;
obs_set->ra = AcTan(sin_alpha,cos_alpha); /* Right Ascension (radians)*/
obs_set->ra = FMod2p(obs_set->ra);
/*}*/ /*if*/
} /* Procedure Calculate_RADec */
/*** FIXME: formalise with other copies */
static void Calc_RADec (gdouble jul_utc, gdouble saz, gdouble sel,
qth_t *qth, obs_astro_t *obs_set)
{
double phi,theta,sin_theta,cos_theta,sin_phi,cos_phi,
az,el,Lxh,Lyh,Lzh,Sx,Ex,Zx,Sy,Ey,Zy,Sz,Ez,Zz,
Lx,Ly,Lz,cos_delta,sin_alpha,cos_alpha;
geodetic_t geodetic;
geodetic.lon = qth->lon * de2ra;
geodetic.lat = qth->lat * de2ra;
geodetic.alt = qth->alt / 1000.0;
geodetic.theta = 0;
az = saz * de2ra;
el = sel * de2ra;
phi = geodetic.lat;
theta = FMod2p(ThetaG_JD(jul_utc) + geodetic.lon);
sin_theta = sin(theta);
cos_theta = cos(theta);
sin_phi = sin(phi);
cos_phi = cos(phi);
Lxh = -cos(az) * cos(el);
Lyh = sin(az) * cos(el);
Lzh = sin(el);
Sx = sin_phi * cos_theta;
Ex = -sin_theta;
Zx = cos_theta * cos_phi;
Sy = sin_phi * sin_theta;
Ey = cos_theta;
Zy = sin_theta*cos_phi;
Sz = -cos_phi;
Ez = 0;
Zz = sin_phi;
Lx = Sx*Lxh + Ex * Lyh + Zx*Lzh;
Ly = Sy*Lxh + Ey * Lyh + Zy*Lzh;
Lz = Sz*Lxh + Ez * Lyh + Zz*Lzh;
obs_set->dec = ArcSin(Lz); /* Declination (radians)*/
cos_delta = sqrt(1 - Sqr(Lz));
sin_alpha = Ly / cos_delta;
cos_alpha = Lx / cos_delta;
obs_set->ra = AcTan(sin_alpha,cos_alpha); /* Right Ascension (radians)*/
obs_set->ra = FMod2p(obs_set->ra);
}
void Calculate_RADec(double time, const double pos[3], const double vel[3], geodetic_t *geodetic, vector_t *obs_set)
{
/* Reference: Methods of Orbit Determination by */
/* Pedro Ramon Escobal, pp. 401-402 */
double phi, theta, sin_theta, cos_theta, sin_phi, cos_phi, az, el,
Lxh, Lyh, Lzh, Sx, Ex, Zx, Sy, Ey, Zy, Sz, Ez, Zz, Lx, Ly,
Lz, cos_delta, sin_alpha, cos_alpha;
Calculate_Obs(time,pos,vel,geodetic,obs_set);
az=obs_set->x;
el=obs_set->y;
phi=geodetic->lat;
theta=FMod2p(ThetaG_JD(time)+geodetic->lon);
sin_theta=sin(theta);
cos_theta=cos(theta);
sin_phi=sin(phi);
cos_phi=cos(phi);
Lxh=-cos(az)*cos(el);
Lyh=sin(az)*cos(el);
Lzh=sin(el);
Sx=sin_phi*cos_theta;
Ex=-sin_theta;
Zx=cos_theta*cos_phi;
Sy=sin_phi*sin_theta;
Ey=cos_theta;
Zy=sin_theta*cos_phi;
Sz=-cos_phi;
Ez=0.0;
Zz=sin_phi;
Lx=Sx*Lxh+Ex*Lyh+Zx*Lzh;
Ly=Sy*Lxh+Ey*Lyh+Zy*Lzh;
Lz=Sz*Lxh+Ez*Lyh+Zz*Lzh;
obs_set->y=ArcSin(Lz); /* Declination (radians) */
cos_delta=sqrt(1.0-Sqr(Lz));
sin_alpha=Ly/cos_delta;
cos_alpha=Lx/cos_delta;
obs_set->x=AcTan(sin_alpha,cos_alpha); /* Right Ascension (radians) */
obs_set->x=FMod2p(obs_set->x);
}
bool cNoradSDP4::DeepPeriodics(double *e, double *xincc,
double *omgadf, double *xnode,
double *xmam, double tsince)
{
// Lunar-solar periodics
double sinis = sin(*xincc);
double cosis = cos(*xincc);
double sghs = 0.0;
double shs = 0.0;
double sh1 = 0.0;
double pe = 0.0;
double pinc = 0.0;
double pl = 0.0;
double sghl = 0.0;
// In SGP4-1980, the lunar-solar terms were recalculated only when the
// propagation time changed by 30 minutes or more in order to save
// CPU effort. This could cause "choppy" ephemerides for some orbits.
// Here the terms are calculated for all propagation times.
// Apply lunar-solar terms
double zm = dp_zmos + zns * tsince;
double zf = zm + 2.0 * zes * sin(zm);
double sinzf = sin(zf);
double f2 = 0.5 * sinzf * sinzf - 0.25;
double f3 = -0.5 * sinzf * cos(zf);
double ses = dp_se2 * f2 + dp_se3 * f3;
double sis = dp_si2 * f2 + dp_si3 * f3;
double sls = dp_sl2 * f2 + dp_sl3 * f3 + dp_sl4 * sinzf;
sghs = dp_sgh2 * f2 + dp_sgh3 * f3 + dp_sgh4 * sinzf;
shs = dp_sh2 * f2 + dp_sh3 * f3;
zm = dp_zmol + znl * tsince;
zf = zm + 2.0 * zel * sin(zm);
sinzf = sin(zf);
f2 = 0.5 * sinzf * sinzf - 0.25;
f3 = -0.5 * sinzf * cos(zf);
double sel = dp_ee2 * f2 + dp_e3 * f3;
double sil = dp_xi2 * f2 + dp_xi3 * f3;
double sll = dp_xl2 * f2 + dp_xl3 * f3 + dp_xl4 * sinzf;
sghl = dp_xgh2 * f2 + dp_xgh3 * f3 + dp_xgh4 * sinzf;
sh1 = dp_xh2 * f2 + dp_xh3 * f3;
pe = ses + sel;
pinc = sis + sil;
pl = sls + sll;
double pgh = sghs + sghl;
double ph = shs + sh1;
*xincc = (*xincc) + pinc;
*e = (*e) + pe;
if (m_Orbit.Inclination() >= 0.2)
{
// Apply periodics directly
ph = ph / m_sinio;
pgh = pgh - m_cosio * ph;
*omgadf = (*omgadf) + pgh;
*xnode = (*xnode) + ph;
*xmam = (*xmam) + pl;
}
else
{
// Apply periodics with Lyddane modification
double sinok = sin(*xnode);
double cosok = cos(*xnode);
double alfdp = sinis * sinok;
double betdp = sinis * cosok;
double dalf = ph * cosok + pinc * cosis * sinok;
double dbet = -ph * sinok + pinc * cosis * cosok;
alfdp = alfdp + dalf;
betdp = betdp + dbet;
double xls = (*xmam) + (*omgadf) + cosis * (*xnode);
double dls = pl + pgh - pinc * (*xnode) * sinis;
xls = xls + dls;
*xnode = AcTan(alfdp, betdp);
*xmam = (*xmam) + pl;
*omgadf = xls - (*xmam) - cos(*xincc) * (*xnode);
}
return true;
}
cNoradSDP4::cNoradSDP4(const cOrbit &orbit) :
cNoradBase(orbit)
{
double sinarg = sin(m_Orbit.ArgPerigee());
double cosarg = cos(m_Orbit.ArgPerigee());
double eqsq = sqr(m_Orbit.Eccentricity());
// Deep space initialization
cJulian jd = m_Orbit.Epoch();
dp_thgr = jd.ToGmst();
double eq = m_Orbit.Eccentricity();
double aqnv = 1.0 / m_Orbit.SemiMajor();
double xmao = m_Orbit.MeanAnomaly();
double xpidot = m_omgdot + m_xnodot;
double sinq = sin(m_Orbit.RAAN());
double cosq = cos(m_Orbit.RAAN());
// Initialize lunar solar terms
double day = jd.FromJan1_12h_1900();
double dpi_xnodce = 4.5236020 - 9.2422029E-4 * day;
double dpi_stem = sin(dpi_xnodce);
double dpi_ctem = cos(dpi_xnodce);
double dpi_zcosil = 0.91375164 - 0.03568096 * dpi_ctem;
double dpi_zsinil = sqrt(1.0 - dpi_zcosil * dpi_zcosil);
double dpi_zsinhl = 0.089683511 *dpi_stem / dpi_zsinil;
double dpi_zcoshl = sqrt(1.0 - dpi_zsinhl * dpi_zsinhl);
double dpi_c = 4.7199672 + 0.22997150 * day;
double dpi_gam = 5.8351514 + 0.0019443680 * day;
dp_zmol = Fmod2p(dpi_c - dpi_gam);
double dpi_zx = 0.39785416 * dpi_stem / dpi_zsinil;
double dpi_zy = dpi_zcoshl * dpi_ctem + 0.91744867 * dpi_zsinhl * dpi_stem;
dpi_zx = AcTan(dpi_zx,dpi_zy) + dpi_gam - dpi_xnodce;
double dpi_zcosgl = cos(dpi_zx);
double dpi_zsingl = sin(dpi_zx);
dp_zmos = 6.2565837 + 0.017201977 * day;
dp_zmos = Fmod2p(dp_zmos);
const double zcosis = 0.91744867;
const double zsinis = 0.39785416;
const double c1ss = 2.9864797e-06;
const double zsings = -0.98088458;
const double zcosgs = 0.1945905;
double zcosg = zcosgs;
double zsing = zsings;
double zcosi = zcosis;
double zsini = zsinis;
double zcosh = cosq;
double zsinh = sinq;
double cc = c1ss;
double zn = zns;
double ze = zes;
double zmo = dp_zmos;
double xnoi = 1.0 / m_Orbit.MeanMotion();
double se = 0.0; double si = 0.0; double sl = 0.0;
double sgh = 0.0; double sh = 0.0;
// Apply the solar and lunar terms on the first pass, then re-apply the
// solar terms again on the second pass.
for (int pass = 1; pass <= 2; pass++)
{
// Do solar terms
double a1 = zcosg * zcosh + zsing * zcosi * zsinh;
double a3 = -zsing * zcosh + zcosg * zcosi * zsinh;
double a7 = -zcosg * zsinh + zsing * zcosi * zcosh;
double a8 = zsing * zsini;
double a9 = zsing * zsinh + zcosg * zcosi * zcosh;
double a10 = zcosg * zsini;
double a2 = m_cosio * a7 + m_sinio * a8;
double a4 = m_cosio * a9 + m_sinio * a10;
double a5 = -m_sinio * a7 + m_cosio * a8;
double a6 = -m_sinio * a9 + m_cosio * a10;
double x1 = a1 * cosarg + a2 * sinarg;
double x2 = a3 * cosarg + a4 * sinarg;
double x3 = -a1 * sinarg + a2 * cosarg;
double x4 = -a3 * sinarg + a4 * cosarg;
double x5 = a5 * sinarg;
double x6 = a6 * sinarg;
double x7 = a5 * cosarg;
double x8 = a6 * cosarg;
double z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3;
double z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4;
double z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4;
double z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * eqsq;
double z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * eqsq;
double z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * eqsq;
double z11 = -6.0 * a1 * a5 + eqsq*(-24.0 * x1 * x7 - 6.0 * x3 * x5);
double z12 = -6.0 * (a1 * a6 + a3 * a5) +
eqsq * (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5));
double z13 = -6.0 * a3 * a6 + eqsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6);
double z21 = 6.0 * a2 * a5 + eqsq * (24.0 * x1 * x5 - 6.0 * x3 * x7);
double z22 = 6.0*(a4 * a5 + a2 * a6) +
eqsq * (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8));
double z23 = 6.0 * a4 * a6 + eqsq*(24.0 * x2 * x6 - 6.0 * x4 * x8);
z1 = z1 + z1 + m_betao2 * z31;
z2 = z2 + z2 + m_betao2 * z32;
z3 = z3 + z3 + m_betao2 * z33;
double s3 = cc * xnoi;
double s2 = -0.5 * s3 / m_betao;
double s4 = s3 * m_betao;
double s1 = -15.0 * eq * s4;
double s5 = x1 * x3 + x2 * x4;
double s6 = x2 * x3 + x1 * x4;
double s7 = x2 * x4 - x1 * x3;
se = s1 * zn * s5;
si = s2 * zn * (z11 + z13);
sl = -zn * s3 * (z1 + z3 - 14.0 - 6.0 * eqsq);
sgh = s4 * zn * (z31 + z33 - 6.0);
sh = -zn * s2 * (z21 + z23);
if (m_Orbit.Inclination() < 5.2359877E-2)
{
sh = 0.0;
}
dp_ee2 = 2.0 * s1 * s6;
dp_e3 = 2.0 * s1 * s7;
dp_xi2 = 2.0 * s2 * z12;
dp_xi3 = 2.0 * s2 * (z13 - z11);
dp_xl2 = -2.0 * s3 * z2;
dp_xl3 = -2.0 * s3 * (z3 - z1);
dp_xl4 = -2.0 * s3 * (-21.0 - 9.0 * eqsq) * ze;
dp_xgh2 = 2.0 * s4 * z32;
dp_xgh3 = 2.0 * s4 * (z33 - z31);
dp_xgh4 = -18.0 * s4 * ze;
dp_xh2 = -2.0 * s2 * z22;
dp_xh3 = -2.0 * s2 * (z23 - z21);
if (pass == 1)
{
// Do lunar terms
dp_sse = se;
dp_ssi = si;
dp_ssl = sl;
dp_ssh = sh / m_sinio;
dp_ssg = sgh - m_cosio * dp_ssh;
dp_se2 = dp_ee2;
dp_si2 = dp_xi2;
dp_sl2 = dp_xl2;
dp_sgh2 = dp_xgh2;
dp_sh2 = dp_xh2;
dp_se3 = dp_e3;
dp_si3 = dp_xi3;
dp_sl3 = dp_xl3;
dp_sgh3 = dp_xgh3;
dp_sh3 = dp_xh3;
dp_sl4 = dp_xl4;
dp_sgh4 = dp_xgh4;
zcosg = dpi_zcosgl;
zsing = dpi_zsingl;
zcosi = dpi_zcosil;
zsini = dpi_zsinil;
zcosh = dpi_zcoshl * cosq + dpi_zsinhl * sinq;
zsinh = sinq * dpi_zcoshl - cosq * dpi_zsinhl;
zn = znl;
const double c1l = 4.7968065e-07;
cc = c1l;
ze = zel;
zmo = dp_zmol;
}
}
dp_sse = dp_sse + se;
dp_ssi = dp_ssi + si;
dp_ssl = dp_ssl + sl;
dp_ssg = dp_ssg + sgh - m_cosio / m_sinio * sh;
dp_ssh = dp_ssh + sh / m_sinio;
// Geopotential resonance initialization
gp_reso = false;
gp_sync = false;
double g310;
double f220;
double bfact = 0.0;
// Determine if orbit is 24- or 12-hour resonant.
// Mean motion is given in radians per minute.
if ((m_Orbit.MeanMotion() > 0.0034906585) && (m_Orbit.MeanMotion() < 0.0052359877))
{
// Orbit is within the Clarke Belt (period is 24-hour resonant).
// Synchronous resonance terms initialization
gp_reso = true;
gp_sync = true;
double g200 = 1.0 + eqsq * (-2.5 + 0.8125 * eqsq);
g310 = 1.0 + 2.0 * eqsq;
double g300 = 1.0 + eqsq * (-6.0 + 6.60937 * eqsq);
f220 = 0.75 * (1.0 + m_cosio) * (1.0 + m_cosio);
double f311 = 0.9375 * m_sinio * m_sinio * (1.0 + 3 * m_cosio) - 0.75 * (1.0 + m_cosio);
double f330 = 1.0 + m_cosio;
const double q22 = 1.7891679e-06;
const double q31 = 2.1460748e-06;
const double q33 = 2.2123015e-07;
f330 = 1.875 * f330 * f330 * f330;
dp_del1 = 3.0 * m_Orbit.MeanMotion() * m_Orbit.MeanMotion() * aqnv * aqnv;
dp_del2 = 2.0 * dp_del1 * f220 * g200 * q22;
dp_del3 = 3.0 * dp_del1 * f330 * g300 * q33 * aqnv;
dp_del1 = dp_del1 * f311 * g310 * q31 * aqnv;
dp_xlamo = xmao + m_Orbit.RAAN() + m_Orbit.ArgPerigee() - dp_thgr;
bfact = m_xmdot + xpidot - thdt;
bfact = bfact + dp_ssl + dp_ssg + dp_ssh;
}
else if (((m_Orbit.MeanMotion() >= 8.26E-03) && (m_Orbit.MeanMotion() <= 9.24E-03)) && (eq >= 0.5))
{
// Period is 12-hour resonant
gp_reso = true;
double eoc = eq * eqsq;
double g201 = -0.306 - (eq - 0.64) * 0.440;
double g211; double g322;
double g410; double g422;
double g520;
if (eq <= 0.65)
{
g211 = 3.616 - 13.247 * eq + 16.290 * eqsq;
g310 = -19.302 + 117.390 * eq - 228.419 * eqsq + 156.591 * eoc;
g322 = -18.9068 + 109.7927 * eq - 214.6334 * eqsq + 146.5816 * eoc;
g410 = -41.122 + 242.694 * eq - 471.094 * eqsq + 313.953 * eoc;
g422 = -146.407 + 841.880 * eq - 1629.014 * eqsq + 1083.435 * eoc;
g520 = -532.114 + 3017.977 * eq - 5740.0 * eqsq + 3708.276 * eoc;
}
else
{
g211 = -72.099 + 331.819 * eq - 508.738 * eqsq + 266.724 * eoc;
g310 = -346.844 + 1582.851 * eq - 2415.925 * eqsq + 1246.113 * eoc;
g322 = -342.585 + 1554.908 * eq - 2366.899 * eqsq + 1215.972 * eoc;
g410 = -1052.797 + 4758.686 * eq - 7193.992 * eqsq + 3651.957 * eoc;
g422 = -3581.69 + 16178.11 * eq - 24462.77 * eqsq + 12422.52 * eoc;
if (eq <= 0.715)
{
g520 = 1464.74 - 4664.75 * eq + 3763.64 * eqsq;
}
else
{
g520 = -5149.66 + 29936.92 * eq - 54087.36 * eqsq + 31324.56 * eoc;
}
}
double g533;
double g521;
double g532;
if (eq < 0.7)
{
g533 = -919.2277 + 4988.61 * eq - 9064.77 * eqsq + 5542.21 * eoc;
g521 = -822.71072 + 4568.6173 * eq - 8491.4146 * eqsq + 5337.524 * eoc;
g532 = -853.666 + 4690.25 * eq - 8624.77 * eqsq + 5341.4 * eoc;
}
else
{
g533 = -37995.78 + 161616.52 * eq - 229838.2 * eqsq + 109377.94 * eoc;
g521 = -51752.104 + 218913.95 * eq - 309468.16 * eqsq + 146349.42 * eoc;
g532 = -40023.88 + 170470.89 * eq - 242699.48 * eqsq + 115605.82 * eoc;
}
double sini2 = sqr(m_sinio);
double theta2 = sqr(m_cosio);
f220 = 0.75 * (1.0 + 2.0 * m_cosio + theta2);
const double root22 = 1.7891679e-06;
const double root32 = 3.7393792e-07;
const double root44 = 7.3636953e-09;
const double root52 = 1.1428639e-07;
const double root54 = 2.1765803e-09;
double f221 = 1.5 * sini2;
double f321 = 1.875 * m_sinio * (1.0 - 2.0 * m_cosio - 3.0 * theta2);
double f322 = -1.875 * m_sinio * (1.0 + 2.0 * m_cosio - 3.0 * theta2);
double f441 = 35.0 * sini2 * f220;
double f442 = 39.3750 * sini2 * sini2;
double f522 = 9.84375 * m_sinio * (sini2 * (1.0 - 2.0 * m_cosio - 5.0 * theta2) +
0.33333333*(-2.0 + 4.0 * m_cosio + 6.0 * theta2));
double f523 = m_sinio * (4.92187512 * sini2 * (-2.0 - 4.0 * m_cosio + 10.0 * theta2) +
6.56250012 * (1.0 + 2.0 * m_cosio - 3.0 * theta2));
double f542 = 29.53125 * m_sinio * ( 2.0 - 8.0 * m_cosio + theta2 * (-12.0 + 8.0 * m_cosio + 10.0 * theta2));
double f543 = 29.53125 * m_sinio * (-2.0 - 8.0 * m_cosio + theta2 * ( 12.0 + 8.0 * m_cosio - 10.0 * theta2));
double xno2 = m_Orbit.MeanMotion() * m_Orbit.MeanMotion();
double ainv2 = aqnv * aqnv;
double temp1 = 3.0 * xno2 * ainv2;
double temp = temp1 * root22;
dp_d2201 = temp * f220 * g201;
dp_d2211 = temp * f221 * g211;
temp1 = temp1 * aqnv;
temp = temp1 * root32;
dp_d3210 = temp * f321 * g310;
dp_d3222 = temp * f322 * g322;
temp1 = temp1 * aqnv;
temp = 2.0 * temp1 * root44;
dp_d4410 = temp * f441 * g410;
dp_d4422 = temp * f442 * g422;
temp1 = temp1 * aqnv;
temp = temp1 * root52;
dp_d5220 = temp * f522 * g520;
dp_d5232 = temp * f523 * g532;
temp = 2.0 * temp1 * root54;
dp_d5421 = temp * f542 * g521;
dp_d5433 = temp * f543 * g533;
dp_xlamo = xmao + m_Orbit.RAAN() + m_Orbit.RAAN() - dp_thgr - dp_thgr;
bfact = m_xmdot + m_xnodot + m_xnodot - thdt - thdt;
bfact = bfact + dp_ssl + dp_ssh + dp_ssh;
}
if (gp_reso || gp_sync)
{
dp_xfact = bfact - m_Orbit.MeanMotion();
// Initialize integrator
dp_xli = dp_xlamo;
dp_xni = m_Orbit.MeanMotion();
dp_atime = 0.0;
dp_stepp = 720.0;
dp_stepn = -720.0;
dp_step2 = 259200.0;
}
else
{
dp_xfact = 0.0;
dp_xli = 0.0;
dp_xni = 0.0;
dp_atime = 0.0;
dp_stepp = 0.0;
dp_stepn = 0.0;
dp_step2 = 0.0;
}
}
void SGP4(float tsince, PredicThirteen::tle_t * tle, PredicThirteen::vector_t * pos, PredicThirteen::vector_t * vel)
{
/* This function is used to calculate the position and velocity */
/* of near-earth (period < 225 minutes) satellites. tsince is */
/* time since epoch in minutes, tle is a pointer to a tle_t */
/* structure with Keplerian orbital elements and pos and vel */
/* are vector_t structures returning ECI satellite position and */
/* velocity. Use Convert_Sat_State() to convert to km and km/s. */
static float aodp, aycof, c1, c4, c5, cosio, d2, d3, d4, delmo,
omgcof, eta, omgdot, sinio, xnodp, sinmo, t2cof, t3cof, t4cof,
t5cof, x1mth2, x3thm1, x7thm1, xmcof, xmdot, xnodcf, xnodot, xlcof;
float cosuk, sinuk, rfdotk, vx, vy, vz, ux, uy, uz, xmy, xmx, cosnok,
sinnok, cosik, sinik, rdotk, xinck, xnodek, uk, rk, cos2u, sin2u,
u, sinu, cosu, betal, rfdot, rdot, r, pl, elsq, esine, ecose, epw,
cosepw, x1m5th, xhdot1, tfour, sinepw, capu, ayn, xlt, aynl, xll,
axn, xn, beta, xl, e, a, tcube, delm, delomg, templ, tempe, tempa,
xnode, tsq, xmp, omega, xnoddf, omgadf, xmdf, a1, a3ovk2, ao,
betao, betao2, c1sq, c2, c3, coef, coef1, del1, delo, eeta, eosq,
etasq, perigee, pinvsq, psisq, qoms24, s4, temp, temp1, temp2,
temp3, temp4, temp5, temp6, theta2, theta4, tsi;
int i;
DEBUG_PRINTLN("----------------------------------------");
printTle(tle);
/* Initialization */
if (isFlagClear(SGP4_INITIALIZED_FLAG))
{
SetFlag(SGP4_INITIALIZED_FLAG);
/* Recover original mean motion (xnodp) and */
/* semimajor axis (aodp) from input elements. */
a1=pow(xke/tle->xno,tothrd);
printVar("a1", a1);
cosio=cos(tle->xincl);
printVar("Tle->xincl", tle->xincl);
theta2=cosio*cosio;
x3thm1=3*theta2-1.0;
printVar("theta2",theta2);
printVar("cosio",cosio);
eosq=tle->eo*tle->eo;
betao2=1.0-eosq;
betao=sqrt(betao2);
del1=1.5*ck2*x3thm1/(a1*a1*betao*betao2);
ao=a1*(1.0-del1*(0.5*tothrd+del1*(1.0+134.0/81.0*del1)));
delo=1.5*ck2*x3thm1/(ao*ao*betao*betao2);
xnodp=tle->xno/(1.0+delo);
aodp=ao/(1.0-delo);
printVar("aodp", aodp);
DEBUG_PRINTLN("----------------------------------------");
/* For perigee less than 220 kilometers, the "simple" */
/* flag is set and the equations are truncated to linear */
/* variation in sqrt a and quadratic variation in mean */
/* anomaly. Also, the c3 term, the delta omega term, and */
/* the delta m term are dropped. */
if ((aodp*(1-tle->eo)/ae)<(220/xkmper+ae))
SetFlag(SIMPLE_FLAG);
else
ClearFlag(SIMPLE_FLAG);
/* For perigees below 156 km, the */
/* values of s and qoms2t are altered. */
s4=s;
qoms24=qoms2t;
perigee=(aodp*(1-tle->eo)-ae)*xkmper;
printVar("perigee", perigee);
if (perigee<156.0)
{
if (perigee<=98.0)
s4=20;
else
s4=perigee-78.0;
qoms24=pow((120-s4)*ae/xkmper,4);
s4=s4/xkmper+ae;
}
DEBUG_PRINTLN("----------------------------------------");
pinvsq=1/(aodp*aodp*betao2*betao2);
tsi=1/(aodp-s4);
eta=aodp*tle->eo*tsi;
etasq=eta*eta;
printVar("etasq",etasq);
eeta=tle->eo*eta;
psisq=fabs(1-etasq);
coef=qoms24*pow(tsi,4);
coef1=coef/pow(psisq,3.5);
printVar("coef1",coef1);
c2=coef1*xnodp*(aodp*(1+1.5*etasq+eeta*(4+etasq))+0.75*ck2*tsi/psisq*x3thm1*(8+3*etasq*(8+etasq)));
printVar("xnodp", xnodp);
printVar("etasq",etasq);
printVar("eeta",eeta);
printVar("ck2",ck2);
printVar("tsi",tsi);
printVar("psisq",psisq);
printVar("x3thm1",x3thm1);
printVar("c2",c2);
c1=tle->bstar*c2;
printVar("c1*1000000",c1*1000000);
printVar("BSTAR: ", tle->bstar);
sinio=sin(tle->xincl);
a3ovk2=-xj3/ck2*pow(ae,3);
c3=coef*tsi*a3ovk2*xnodp*ae*sinio/tle->eo;
x1mth2=1-theta2;
printVar("x1mth2", x1mth2);
DEBUG_PRINTLN("----------------------------------------");
DEBUG_PRINTLN("----------------------------------------");
c4=2*xnodp*coef1*aodp*betao2*(eta*(2+0.5*etasq)+tle->eo*(0.5+2*etasq)-2*ck2*tsi/(aodp*psisq)*(-3*x3thm1*(1-2*eeta+etasq*(1.5-0.5*eeta))+0.75*x1mth2*(2*etasq-eeta*(1+etasq))*cos(2*tle->omegao)));
c5=2*coef1*aodp*betao2*(1+2.75*(etasq+eeta)+eeta*etasq);
theta4=theta2*theta2;
temp1=3*ck2*pinvsq*xnodp;
temp2=temp1*ck2*pinvsq;
temp3=1.25*ck4*pinvsq*pinvsq*xnodp;
xmdot=xnodp+0.5*temp1*betao*x3thm1+0.0625*temp2*betao*(13-78*theta2+137*theta4);
x1m5th=1-5*theta2;
omgdot=-0.5*temp1*x1m5th+0.0625*temp2*(7-114*theta2+395*theta4)+temp3*(3-36*theta2+49*theta4);
xhdot1=-temp1*cosio;
xnodot=xhdot1+(0.5*temp2*(4-19*theta2)+2*temp3*(3-7*theta2))*cosio;
omgcof=tle->bstar*c3*cos(tle->omegao);
xmcof=-tothrd*coef*tle->bstar*ae/eeta;
xnodcf=3.5*betao2*xhdot1*c1;
t2cof=1.5*c1;
printVar("t2cof*1000000", t2cof*1000000);
xlcof=0.125*a3ovk2*sinio*(3+5*cosio)/(1+cosio);
aycof=0.25*a3ovk2*sinio;
delmo=pow(1+eta*cos(tle->xmo),3);
sinmo=sin(tle->xmo);
x7thm1=7*theta2-1;
printVar("x7thm1", x7thm1);
if (isFlagClear(SIMPLE_FLAG))
{
c1sq=c1*c1;
d2=4*aodp*tsi*c1sq;
temp=d2*tsi*c1/3;
d3=(17*aodp+s4)*temp;
d4=0.5*temp*aodp*tsi*(221*aodp+31*s4)*c1;
t3cof=d2+2*c1sq;
t4cof=0.25*(3*d3+c1*(12*d2+10*c1sq));
t5cof=0.2*(3*d4+12*c1*d3+6*d2*d2+15*c1sq*(2*d2+c1sq));
}
}
/* Update for secular gravity and atmospheric drag. */
xmdf=tle->xmo+xmdot*tsince;
omgadf=tle->omegao+omgdot*tsince;
xnoddf=tle->xnodeo+xnodot*tsince;
omega=omgadf;
xmp=xmdf;
tsq=tsince*tsince;
printVar("tsince*1000000", tsince*1000000);
printVar("tsq", tsq);
xnode=xnoddf+xnodcf*tsq;
tempa=1-c1*tsince;
tempe=tle->bstar*c4*tsince;
templ=t2cof*tsq;
printVar("templ", templ);
if (isFlagClear(SIMPLE_FLAG))
{
delomg=omgcof*tsince;
delm=xmcof*(pow(1+eta*cos(xmdf),3)-delmo);
temp=delomg+delm;
xmp=xmdf+temp;
omega=omgadf-temp;
tcube=tsq*tsince;
tfour=tsince*tcube;
tempa=tempa-d2*tsq-d3*tcube-d4*tfour;
tempe=tempe+tle->bstar*c5*(sin(xmp)-sinmo);
templ=templ+t3cof*tcube+tfour*(t4cof+tsince*t5cof);
printVar("templ", templ);
}
a=aodp*pow(tempa,2);
e=tle->eo-tempe;
xl=xmp+omega+xnode+xnodp*templ;
beta=sqrt(1-e*e);
xn=xke/pow(a,1.5);
/* Long period periodics */
axn=e*cos(omega);
temp=1/(a*beta*beta);
xll=temp*xlcof*axn;
aynl=temp*aycof;
xlt=xl+xll;
ayn=e*sin(omega)+aynl;
/* Solve Kepler's Equation */
capu=FMod2p(xlt-xnode);
temp2=capu;
i=0;
do
{
sinepw=sin(temp2);
cosepw=cos(temp2);
temp3=axn*sinepw;
temp4=ayn*cosepw;
temp5=axn*cosepw;
temp6=ayn*sinepw;
epw=(capu-temp4+temp3-temp2)/(1-temp5-temp6)+temp2;
if (fabs(epw-temp2)<= e6a)
break;
temp2=epw;
} while (i++<10);
/* Short p2eriod preliminary quantities */
ecose=temp5+temp6;
esine=temp3-temp4;
elsq=axn*axn+ayn*ayn;
temp=1-elsq;
pl=a*temp;
r=a*(1-ecose);
temp1=1/r;
rdot=xke*sqrt(a)*esine*temp1;
rfdot=xke*sqrt(pl)*temp1;
temp2=a*temp1;
betal=sqrt(temp);
temp3=1/(1+betal);
cosu=temp2*(cosepw-axn+ayn*esine*temp3);
sinu=temp2*(sinepw-ayn-axn*esine*temp3);
u=AcTan(sinu,cosu);
sin2u=2*sinu*cosu;
cos2u=2*cosu*cosu-1;
temp=1/pl;
temp1=ck2*temp;
temp2=temp1*temp;
/* Update for short periodics */
rk=r*(1-1.5*temp2*betal*x3thm1)+0.5*temp1*x1mth2*cos2u;
uk=u-0.25*temp2*x7thm1*sin2u;
xnodek=xnode+1.5*temp2*cosio*sin2u;
xinck=tle->xincl+1.5*temp2*cosio*sinio*cos2u;
rdotk=rdot-xn*temp1*x1mth2*sin2u;
rfdotk=rfdot+xn*temp1*(x1mth2*cos2u+1.5*x3thm1);
/* Orientation vectors */
sinuk=sin(uk);
cosuk=cos(uk);
sinik=sin(xinck);
cosik=cos(xinck);
sinnok=sin(xnodek);
cosnok=cos(xnodek);
xmx=-sinnok*cosik;
xmy=cosnok*cosik;
ux=xmx*sinuk+cosnok*cosuk;
uy=xmy*sinuk+sinnok*cosuk;
uz=sinik*sinuk;
vx=xmx*cosuk-cosnok*sinuk;
vy=xmy*cosuk-sinnok*sinuk;
vz=sinik*cosuk;
/* Position and velocity */
pos->x=rk*ux;
pos->y=rk*uy;
pos->z=rk*uz;
vel->x=rdotk*ux+rfdotk*vx;
vel->y=rdotk*uy+rfdotk*vy;
vel->z=rdotk*uz+rfdotk*vz;
DEBUG_PRINTLN("----------------------------------------");
DEBUG_PRINT("Position: ");
printVector(pos);
DEBUG_PRINT("Velocity; ");
printVector(vel);
/* Phase in radians */
phase=xlt-xnode-omgadf+twopi;
if (phase<0.0)
phase+=twopi;
phase=FMod2p(phase);
}
void sgp4_predict(struct _sgp4 *m, double tsince, tle_t *tle, double pos[3], double vel[3])
{
/* This function is used to calculate the position and velocity */
/* of near-earth (period < 225 minutes) satellites. tsince is */
/* time since epoch in minutes, tle is a pointer to a tle_t */
/* structure with Keplerian orbital elements and pos and vel */
/* are vector_t structures returning ECI satellite position and */
/* velocity. Use Convert_Sat_State() to convert to km and km/s. */
double cosuk, sinuk, rfdotk, vx, vy, vz, ux, uy, uz, xmy, xmx, cosnok,
sinnok, cosik, sinik, rdotk, xinck, xnodek, uk, rk, cos2u, sin2u,
u, sinu, cosu, betal, rfdot, rdot, r, pl, elsq, esine, ecose, epw,
cosepw, x1m5th, xhdot1, tfour, sinepw, capu, ayn, xlt, aynl, xll,
axn, xn, beta, xl, e, a, tcube, delm, delomg, templ, tempe, tempa,
xnode, tsq, xmp, omega, xnoddf, omgadf, xmdf, a1, a3ovk2, ao,
betao, betao2, c1sq, c2, c3, coef, coef1, del1, delo, eeta, eosq,
etasq, perigee, pinvsq, psisq, qoms24, s4, temp, temp1, temp2,
temp3, temp4, temp5, temp6, theta2, theta4, tsi;
int i;
/* Initialization */
if (!m->initialized) {
//Set initialized flag:
m->initialized = true;
/* Recover original mean motion (m->xnodp) and */
/* semimajor axis (m->aodp) from input elements. */
a1=pow(xke/tle->xno,tothrd);
m->cosio=cos(tle->xincl);
theta2=m->cosio*m->cosio;
m->x3thm1=3*theta2-1.0;
eosq=tle->eo*tle->eo;
betao2=1.0-eosq;
betao=sqrt(betao2);
del1=1.5*ck2*m->x3thm1/(a1*a1*betao*betao2);
ao=a1*(1.0-del1*(0.5*tothrd+del1*(1.0+134.0/81.0*del1)));
delo=1.5*ck2*m->x3thm1/(ao*ao*betao*betao2);
m->xnodp=tle->xno/(1.0+delo);
m->aodp=ao/(1.0-delo);
/* For perigee less than 220 kilometers, the "simple" */
/* flag is set and the equations are truncated to linear */
/* variation in sqrt a and quadratic variation in mean */
/* anomaly. Also, the c3 term, the delta omega term, and */
/* the delta m term are dropped. */
if ((m->aodp*(1-tle->eo)/ae)<(220/xkmper+ae))
m->simpleFlag = true;
else
m->simpleFlag = false;
/* For perigees below 156 km, the */
/* values of s and qoms2t are altered. */
s4=s;
qoms24=qoms2t;
perigee=(m->aodp*(1-tle->eo)-ae)*xkmper;
if (perigee<156.0)
{
if (perigee<=98.0)
s4=20;
else
s4=perigee-78.0;
qoms24=pow((120-s4)*ae/xkmper,4);
s4=s4/xkmper+ae;
}
pinvsq=1/(m->aodp*m->aodp*betao2*betao2);
tsi=1/(m->aodp-s4);
m->eta=m->aodp*tle->eo*tsi;
etasq=m->eta*m->eta;
eeta=tle->eo*m->eta;
psisq=fabs(1-etasq);
coef=qoms24*pow(tsi,4);
coef1=coef/pow(psisq,3.5);
c2=coef1*m->xnodp*(m->aodp*(1+1.5*etasq+eeta*(4+etasq))+0.75*ck2*tsi/psisq*m->x3thm1*(8+3*etasq*(8+etasq)));
m->c1=tle->bstar*c2;
m->sinio=sin(tle->xincl);
a3ovk2=-xj3/ck2*pow(ae,3);
c3=coef*tsi*a3ovk2*m->xnodp*ae*m->sinio/tle->eo;
m->x1mth2=1-theta2;
m->c4=2*m->xnodp*coef1*m->aodp*betao2*(m->eta*(2+0.5*etasq)+tle->eo*(0.5+2*etasq)-2*ck2*tsi/(m->aodp*psisq)*(-3*m->x3thm1*(1-2*eeta+etasq*(1.5-0.5*eeta))+0.75*m->x1mth2*(2*etasq-eeta*(1+etasq))*cos(2*tle->omegao)));
m->c5=2*coef1*m->aodp*betao2*(1+2.75*(etasq+eeta)+eeta*etasq);
theta4=theta2*theta2;
temp1=3*ck2*pinvsq*m->xnodp;
temp2=temp1*ck2*pinvsq;
temp3=1.25*ck4*pinvsq*pinvsq*m->xnodp;
m->xmdot=m->xnodp+0.5*temp1*betao*m->x3thm1+0.0625*temp2*betao*(13-78*theta2+137*theta4);
x1m5th=1-5*theta2;
m->omgdot=-0.5*temp1*x1m5th+0.0625*temp2*(7-114*theta2+395*theta4)+temp3*(3-36*theta2+49*theta4);
xhdot1=-temp1*m->cosio;
m->xnodot=xhdot1+(0.5*temp2*(4-19*theta2)+2*temp3*(3-7*theta2))*m->cosio;
m->omgcof=tle->bstar*c3*cos(tle->omegao);
m->xmcof=-tothrd*coef*tle->bstar*ae/eeta;
m->xnodcf=3.5*betao2*xhdot1*m->c1;
m->t2cof=1.5*m->c1;
m->xlcof=0.125*a3ovk2*m->sinio*(3+5*m->cosio)/(1+m->cosio);
m->aycof=0.25*a3ovk2*m->sinio;
m->delmo=pow(1+m->eta*cos(tle->xmo),3);
m->sinmo=sin(tle->xmo);
m->x7thm1=7*theta2-1;
if (!m->simpleFlag) {
c1sq=m->c1*m->c1;
m->d2=4*m->aodp*tsi*c1sq;
temp=m->d2*tsi*m->c1/3;
m->d3=(17*m->aodp+s4)*temp;
m->d4=0.5*temp*m->aodp*tsi*(221*m->aodp+31*s4)*m->c1;
m->t3cof=m->d2+2*c1sq;
m->t4cof=0.25*(3*m->d3+m->c1*(12*m->d2+10*c1sq));
m->t5cof=0.2*(3*m->d4+12*m->c1*m->d3+6*m->d2*m->d2+15*c1sq*(2*m->d2+c1sq));
}
}
/* Update for secular gravity and atmospheric drag. */
xmdf=tle->xmo+m->xmdot*tsince;
omgadf=tle->omegao+m->omgdot*tsince;
xnoddf=tle->xnodeo+m->xnodot*tsince;
omega=omgadf;
xmp=xmdf;
tsq=tsince*tsince;
xnode=xnoddf+m->xnodcf*tsq;
tempa=1-m->c1*tsince;
tempe=tle->bstar*m->c4*tsince;
templ=m->t2cof*tsq;
if (!m->simpleFlag) {
delomg=m->omgcof*tsince;
delm=m->xmcof*(pow(1+m->eta*cos(xmdf),3)-m->delmo);
temp=delomg+delm;
xmp=xmdf+temp;
omega=omgadf-temp;
tcube=tsq*tsince;
tfour=tsince*tcube;
tempa=tempa-m->d2*tsq-m->d3*tcube-m->d4*tfour;
tempe=tempe+tle->bstar*m->c5*(sin(xmp)-m->sinmo);
templ=templ+m->t3cof*tcube+tfour*(m->t4cof+tsince*m->t5cof);
}
a=m->aodp*pow(tempa,2);
e=tle->eo-tempe;
xl=xmp+omega+xnode+m->xnodp*templ;
beta=sqrt(1-e*e);
xn=xke/pow(a,1.5);
/* Long period periodics */
axn=e*cos(omega);
temp=1/(a*beta*beta);
xll=temp*m->xlcof*axn;
aynl=temp*m->aycof;
xlt=xl+xll;
ayn=e*sin(omega)+aynl;
/* Solve Kepler's Equation */
capu=FMod2p(xlt-xnode);
temp2=capu;
i=0;
do
{
sinepw=sin(temp2);
cosepw=cos(temp2);
temp3=axn*sinepw;
temp4=ayn*cosepw;
temp5=axn*cosepw;
temp6=ayn*sinepw;
epw=(capu-temp4+temp3-temp2)/(1-temp5-temp6)+temp2;
if (fabs(epw-temp2)<= e6a)
break;
temp2=epw;
} while (i++<10);
/* Short period preliminary quantities */
ecose=temp5+temp6;
esine=temp3-temp4;
elsq=axn*axn+ayn*ayn;
temp=1-elsq;
pl=a*temp;
r=a*(1-ecose);
temp1=1/r;
rdot=xke*sqrt(a)*esine*temp1;
rfdot=xke*sqrt(pl)*temp1;
temp2=a*temp1;
betal=sqrt(temp);
temp3=1/(1+betal);
cosu=temp2*(cosepw-axn+ayn*esine*temp3);
sinu=temp2*(sinepw-ayn-axn*esine*temp3);
u=AcTan(sinu,cosu);
sin2u=2*sinu*cosu;
cos2u=2*cosu*cosu-1;
temp=1/pl;
temp1=ck2*temp;
temp2=temp1*temp;
/* Update for short periodics */
rk=r*(1-1.5*temp2*betal*m->x3thm1)+0.5*temp1*m->x1mth2*cos2u;
uk=u-0.25*temp2*m->x7thm1*sin2u;
xnodek=xnode+1.5*temp2*m->cosio*sin2u;
xinck=tle->xincl+1.5*temp2*m->cosio*m->sinio*cos2u;
rdotk=rdot-xn*temp1*m->x1mth2*sin2u;
rfdotk=rfdot+xn*temp1*(m->x1mth2*cos2u+1.5*m->x3thm1);
/* Orientation vectors */
sinuk=sin(uk);
cosuk=cos(uk);
sinik=sin(xinck);
cosik=cos(xinck);
sinnok=sin(xnodek);
cosnok=cos(xnodek);
xmx=-sinnok*cosik;
xmy=cosnok*cosik;
ux=xmx*sinuk+cosnok*cosuk;
uy=xmy*sinuk+sinnok*cosuk;
uz=sinik*sinuk;
vx=xmx*cosuk-cosnok*sinuk;
vy=xmy*cosuk-sinnok*sinuk;
vz=sinik*cosuk;
/* Position and velocity */
pos[0] = rk*ux;
pos[1] = rk*uy;
pos[2] = rk*uz;
vel[0] = rdotk*ux+rfdotk*vx;
vel[1] = rdotk*uy+rfdotk*vy;
vel[2] = rdotk*uz+rfdotk*vz;
/* Phase in radians */
m->phase=xlt-xnode-omgadf+twopi;
if (m->phase<0.0)
m->phase+=twopi;
m->phase=FMod2p(m->phase);
}
bool cNoradBase::FinalPosition(double incl, double omega,
double e, double a,
double xl, double xnode,
double xn, double tsince,
cEci &eci)
{
if ((e * e) > 1.0)
{
// error in satellite data
return false;
}
double beta = sqrt(1.0 - e * e);
// Long period periodics
double axn = e * cos(omega);
double temp = 1.0 / (a * beta * beta);
double xll = temp * m_xlcof * axn;
double aynl = temp * m_aycof;
double xlt = xl + xll;
double ayn = e * sin(omega) + aynl;
// Solve Kepler's Equation
double capu = Fmod2p(xlt - xnode);
double temp2 = capu;
double temp3 = 0.0;
double temp4 = 0.0;
double temp5 = 0.0;
double temp6 = 0.0;
double sinepw = 0.0;
double cosepw = 0.0;
bool fDone = false;
for (int i = 1; (i <= 10) && !fDone; i++)
{
sinepw = sin(temp2);
cosepw = cos(temp2);
temp3 = axn * sinepw;
temp4 = ayn * cosepw;
temp5 = axn * cosepw;
temp6 = ayn * sinepw;
double epw = (capu - temp4 + temp3 - temp2) /
(1.0 - temp5 - temp6) + temp2;
if (fabs(epw - temp2) <= E6A)
fDone = true;
else
temp2 = epw;
}
// Short period preliminary quantities
double ecose = temp5 + temp6;
double esine = temp3 - temp4;
double elsq = axn * axn + ayn * ayn;
temp = 1.0 - elsq;
double pl = a * temp;
double r = a * (1.0 - ecose);
double temp1 = 1.0 / r;
double rdot = XKE * sqrt(a) * esine * temp1;
double rfdot = XKE * sqrt(pl) * temp1;
temp2 = a * temp1;
double betal = sqrt(temp);
temp3 = 1.0 / (1.0 + betal);
double cosu = temp2 * (cosepw - axn + ayn * esine * temp3);
double sinu = temp2 * (sinepw - ayn - axn * esine * temp3);
double u = AcTan(sinu, cosu);
double sin2u = 2.0 * sinu * cosu;
double cos2u = 2.0 * cosu * cosu - 1.0;
temp = 1.0 / pl;
temp1 = CK2 * temp;
temp2 = temp1 * temp;
// Update for short periodics
double rk = r * (1.0 - 1.5 * temp2 * betal * m_x3thm1) +
0.5 * temp1 * m_x1mth2 * cos2u;
double uk = u - 0.25 * temp2 * m_x7thm1 * sin2u;
double xnodek = xnode + 1.5 * temp2 * m_cosio * sin2u;
double xinck = incl + 1.5 * temp2 * m_cosio * m_sinio * cos2u;
double rdotk = rdot - xn * temp1 * m_x1mth2 * sin2u;
double rfdotk = rfdot + xn * temp1 * (m_x1mth2 * cos2u + 1.5 * m_x3thm1);
// Orientation vectors
double sinuk = sin(uk);
double cosuk = cos(uk);
double sinik = sin(xinck);
double cosik = cos(xinck);
double sinnok = sin(xnodek);
double cosnok = cos(xnodek);
double xmx = -sinnok * cosik;
double xmy = cosnok * cosik;
double ux = xmx * sinuk + cosnok * cosuk;
double uy = xmy * sinuk + sinnok * cosuk;
double uz = sinik * sinuk;
double vx = xmx * cosuk - cosnok * sinuk;
double vy = xmy * cosuk - sinnok * sinuk;
double vz = sinik * cosuk;
// Position
double x = rk * ux;
double y = rk * uy;
double z = rk * uz;
cVector vecPos(x, y, z);
// Validate on altitude
double altKm = (vecPos.Magnitude() * (XKMPER_WGS72 / AE));
if ((altKm < XKMPER_WGS72) || (altKm > (2 * GEOSYNC_ALT)))
return false;
// Velocity
double xdot = rdotk * ux + rfdotk * vx;
double ydot = rdotk * uy + rfdotk * vy;
double zdot = rdotk * uz + rfdotk * vz;
cVector vecVel(xdot, ydot, zdot);
cJulian gmt = m_Orbit.Epoch();
gmt.addMin(tsince);
eci = cEci(vecPos, vecVel, gmt);
return true;
}
