yodaUtility/sgp4/cNoradBase.cpp

//
// cNoradBase.cpp
//
// Historical Note:
// The equations used here (and in derived classes) to determine satellite 
// ECI coordinates/velocity come from the December, 1980 NORAD document 
// "Space Track Report No. 3". The report details 6 orbital models and 
// provides FORTRAN IV implementations of each. The classes here 
// implement only two of the orbital models: SGP4 and SDP4. These two models, 
// one for "near-earth" objects and one for "deep space" objects, are widely
// used in satellite tracking software and can produce very accurate results
// when used with current NORAD two-line element datum.
//
// The NORAD FORTRAN IV SGP4/SDP4 implementations were converted to Pascal by
// Dr. TS Kelso in 1995. In 1996 these routines were ported in a straight-
// forward manner to C++ by Varol Okan. The SGP4/SDP4 classes here were 
// written by Michael F. Henry in 2002-03 and are a modern C++ re-write of 
// the work done by Okan. In addition to introducing an object-oriented 
// architecture, the last residues of the original FORTRAN code (such as
// labels and gotos) were eradicated.
//
// For excellent information on the underlying physics of orbits, visible 
// satellite observations, current NORAD TLE data, and other related material,
// see http://www.celestrak.com which is maintained by Dr. TS Kelso.
//
// Copyright (c) 2003 Michael F. Henry
//
// mfh 12/07/2003
//
#include "stdafx.h"
#include "cNoradBase.h"
#include "cOrbit.h"
#include "coord.h"
#include "cEci.h"
#include "cVector.h"
#include "cJulian.h"

//////////////////////////////////////////////////////////////////////////////
cNoradBase::cNoradBase(const cOrbit &orbit) :
   m_Orbit(orbit)
{
   Initialize();
}

cNoradBase::~cNoradBase(void)
{
}

cNoradBase& cNoradBase::operator=(const cNoradBase &b)
{
   // m_Orbit is a "const" member var, so cast away its
   // "const-ness" in order to complete the assigment.
   *(const_cast<cOrbit*>(&m_Orbit)) = b.m_Orbit;

   return *this;
}

//////////////////////////////////////////////////////////////////////////////
// Initialize()
// Perform the initialization of member variables, specifically the variables
// used by derived-class objects to calculate ECI coordinates.
void cNoradBase::Initialize()
{
   // Initialize any variables which are time-independent when
   // calculating the ECI coordinates of the satellite.
   m_satInc = m_Orbit.Inclination();
   m_satEcc = m_Orbit.Eccentricity();

   m_cosio  = cos(m_satInc);
   m_theta2 = m_cosio * m_cosio;
   m_x3thm1 = 3.0 * m_theta2 - 1.0;
   m_eosq   = m_satEcc * m_satEcc;
   m_betao2 = 1.0 - m_eosq;
   m_betao  = sqrt(m_betao2);

   // The "recovered" semi-minor axis and mean motion.
   m_aodp  = m_Orbit.SemiMinor();   
   m_xnodp = m_Orbit.mnMotionRec();

   // For perigee below 156 km, the values of S and QOMS2T are altered.
   m_perigee = XKMPER_WGS72 * (m_aodp * (1.0 - m_satEcc) - AE);

   m_s4      = S;
   m_qoms24  = QOMS2T;

   if (m_perigee < 156.0)
   {
      m_s4 = m_perigee - 78.0;

      if (m_perigee <= 98.0)
      {
         m_s4 = 20.0;
      }

      m_qoms24 = pow((120.0 - m_s4) * AE / XKMPER_WGS72, 4.0);
      m_s4 = m_s4 / XKMPER_WGS72 + AE;
   }

   const double pinvsq = 1.0 / (m_aodp * m_aodp * m_betao2 * m_betao2);

   m_tsi   = 1.0 / (m_aodp - m_s4);
   m_eta   = m_aodp * m_satEcc * m_tsi;
   m_etasq = m_eta * m_eta;
   m_eeta  = m_satEcc * m_eta;

   const double psisq  = fabs(1.0 - m_etasq);

   m_coef  = m_qoms24 * pow(m_tsi,4.0);
   m_coef1 = m_coef / pow(psisq,3.5);

   const double c2 = m_coef1 * m_xnodp * 
                     (m_aodp * (1.0 + 1.5 * m_etasq + m_eeta * (4.0 + m_etasq)) +
                     0.75 * CK2 * m_tsi / psisq * m_x3thm1 * 
                     (8.0 + 3.0 * m_etasq * (8.0 + m_etasq)));

   m_c1    = m_Orbit.BStar() * c2;
   m_sinio = sin(m_satInc);

   const double a3ovk2 = -XJ3 / CK2 * pow(AE,3.0);

   m_c3     = m_coef * m_tsi * a3ovk2 * m_xnodp * AE * m_sinio / m_satEcc;
   m_x1mth2 = 1.0 - m_theta2;
   m_c4     = 2.0 * m_xnodp * m_coef1 * m_aodp * m_betao2 * 
              (m_eta * (2.0 + 0.5 * m_etasq) +
              m_satEcc * (0.5 + 2.0 * m_etasq) - 
              2.0 * CK2 * m_tsi / (m_aodp * psisq) *
              (-3.0 * m_x3thm1 * (1.0 - 2.0 * m_eeta + m_etasq * (1.5 - 0.5 * m_eeta)) +
              0.75 * m_x1mth2 * 
              (2.0 * m_etasq - m_eeta * (1.0 + m_etasq)) * 
              cos(2.0 * m_Orbit.ArgPerigee())));

   const double theta4 = m_theta2 * m_theta2;
   const double temp1  = 3.0 * CK2 * pinvsq * m_xnodp;
   const double temp2  = temp1 * CK2 * pinvsq;
   const double temp3  = 1.25 * CK4 * pinvsq * pinvsq * m_xnodp;

   m_xmdot = m_xnodp + 0.5 * temp1 * m_betao * m_x3thm1 +
             0.0625 * temp2 * m_betao * 
             (13.0 - 78.0 * m_theta2 + 137.0 * theta4);

   const double x1m5th = 1.0 - 5.0 * m_theta2;

   m_omgdot = -0.5 * temp1 * x1m5th + 0.0625 * temp2 * 
              (7.0 - 114.0 * m_theta2 + 395.0 * theta4) +
              temp3 * (3.0 - 36.0 * m_theta2 + 49.0 * theta4);

   const double xhdot1 = -temp1 * m_cosio;

   m_xnodot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * m_theta2) +
              2.0 * temp3 * (3.0 - 7.0 * m_theta2)) * m_cosio;
   m_xnodcf = 3.5 * m_betao2 * xhdot1 * m_c1;
   m_t2cof  = 1.5 * m_c1;
   m_xlcof  = 0.125 * a3ovk2 * m_sinio * 
              (3.0 + 5.0 * m_cosio) / (1.0 + m_cosio);
   m_aycof  = 0.25 * a3ovk2 * m_sinio;
   m_x7thm1 = 7.0 * m_theta2 - 1.0;
}

//////////////////////////////////////////////////////////////////////////////
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;
}


1	//
2	// cNoradBase.cpp
3	//
4	// Historical Note:
5	// The equations used here (and in derived classes) to determine satellite
6	// ECI coordinates/velocity come from the December, 1980 NORAD document
7	// "Space Track Report No. 3". The report details 6 orbital models and
8	// provides FORTRAN IV implementations of each. The classes here
9	// implement only two of the orbital models: SGP4 and SDP4. These two models,
10	// one for "near-earth" objects and one for "deep space" objects, are widely
11	// used in satellite tracking software and can produce very accurate results
12	// when used with current NORAD two-line element datum.
13	//
14	// The NORAD FORTRAN IV SGP4/SDP4 implementations were converted to Pascal by
15	// Dr. TS Kelso in 1995. In 1996 these routines were ported in a straight-
16	// forward manner to C++ by Varol Okan. The SGP4/SDP4 classes here were
17	// written by Michael F. Henry in 2002-03 and are a modern C++ re-write of
18	// the work done by Okan. In addition to introducing an object-oriented
19	// architecture, the last residues of the original FORTRAN code (such as
20	// labels and gotos) were eradicated.
21	//
22	// For excellent information on the underlying physics of orbits, visible
23	// satellite observations, current NORAD TLE data, and other related material,
24	// see http://www.celestrak.com which is maintained by Dr. TS Kelso.
25	//
26	// Copyright (c) 2003 Michael F. Henry
27	//
28	// mfh 12/07/2003
29	//
30	#include "stdafx.h"
31	#include "cNoradBase.h"
32	#include "cOrbit.h"
33	#include "coord.h"
34	#include "cEci.h"
35	#include "cVector.h"
36	#include "cJulian.h"
37
38	//////////////////////////////////////////////////////////////////////////////
39	cNoradBase::cNoradBase(const cOrbit &orbit) :
40	m_Orbit(orbit)
41	{
42	Initialize();
43	}
44
45	cNoradBase::~cNoradBase(void)
46	{
47	}
48
49	cNoradBase& cNoradBase::operator=(const cNoradBase &b)
50	{
51	// m_Orbit is a "const" member var, so cast away its
52	// "const-ness" in order to complete the assigment.
53	(const_cast<cOrbit>(&m_Orbit)) = b.m_Orbit;
54
55	return *this;
56	}
57
58	//////////////////////////////////////////////////////////////////////////////
59	// Initialize()
60	// Perform the initialization of member variables, specifically the variables
61	// used by derived-class objects to calculate ECI coordinates.
62	void cNoradBase::Initialize()
63	{
64	// Initialize any variables which are time-independent when
65	// calculating the ECI coordinates of the satellite.
66	m_satInc = m_Orbit.Inclination();
67	m_satEcc = m_Orbit.Eccentricity();
68
69	m_cosio = cos(m_satInc);
70	m_theta2 = m_cosio * m_cosio;
71	m_x3thm1 = 3.0 * m_theta2 - 1.0;
72	m_eosq = m_satEcc * m_satEcc;
73	m_betao2 = 1.0 - m_eosq;
74	m_betao = sqrt(m_betao2);
75
76	// The "recovered" semi-minor axis and mean motion.
77	m_aodp = m_Orbit.SemiMinor();
78	m_xnodp = m_Orbit.mnMotionRec();
79
80	// For perigee below 156 km, the values of S and QOMS2T are altered.
81	m_perigee = XKMPER_WGS72 * (m_aodp * (1.0 - m_satEcc) - AE);
82
83	m_s4 = S;
84	m_qoms24 = QOMS2T;
85
86	if (m_perigee < 156.0)
87	{
88	m_s4 = m_perigee - 78.0;
89
90	if (m_perigee <= 98.0)
91	{
92	m_s4 = 20.0;
93	}
94
95	m_qoms24 = pow((120.0 - m_s4) * AE / XKMPER_WGS72, 4.0);
96	m_s4 = m_s4 / XKMPER_WGS72 + AE;
97	}
98
99	const double pinvsq = 1.0 / (m_aodp * m_aodp * m_betao2 * m_betao2);
100
101	m_tsi = 1.0 / (m_aodp - m_s4);
102	m_eta = m_aodp * m_satEcc * m_tsi;
103	m_etasq = m_eta * m_eta;
104	m_eeta = m_satEcc * m_eta;
105
106	const double psisq = fabs(1.0 - m_etasq);
107
108	m_coef = m_qoms24 * pow(m_tsi,4.0);
109	m_coef1 = m_coef / pow(psisq,3.5);
110
111	const double c2 = m_coef1 * m_xnodp *
112	(m_aodp * (1.0 + 1.5 * m_etasq + m_eeta * (4.0 + m_etasq)) +
113	0.75 * CK2 * m_tsi / psisq * m_x3thm1 *
114	(8.0 + 3.0 * m_etasq * (8.0 + m_etasq)));
115
116	m_c1 = m_Orbit.BStar() * c2;
117	m_sinio = sin(m_satInc);
118
119	const double a3ovk2 = -XJ3 / CK2 * pow(AE,3.0);
120
121	m_c3 = m_coef * m_tsi * a3ovk2 * m_xnodp * AE * m_sinio / m_satEcc;
122	m_x1mth2 = 1.0 - m_theta2;
123	m_c4 = 2.0 * m_xnodp * m_coef1 * m_aodp * m_betao2 *
124	(m_eta * (2.0 + 0.5 * m_etasq) +
125	m_satEcc * (0.5 + 2.0 * m_etasq) -
126	2.0 * CK2 * m_tsi / (m_aodp * psisq) *
127	(-3.0 * m_x3thm1 * (1.0 - 2.0 * m_eeta + m_etasq * (1.5 - 0.5 * m_eeta)) +
128	0.75 * m_x1mth2 *
129	(2.0 * m_etasq - m_eeta * (1.0 + m_etasq)) *
130	cos(2.0 * m_Orbit.ArgPerigee())));
131
132	const double theta4 = m_theta2 * m_theta2;
133	const double temp1 = 3.0 * CK2 * pinvsq * m_xnodp;
134	const double temp2 = temp1 * CK2 * pinvsq;
135	const double temp3 = 1.25 * CK4 * pinvsq * pinvsq * m_xnodp;
136
137	m_xmdot = m_xnodp + 0.5 * temp1 * m_betao * m_x3thm1 +
138	0.0625 * temp2 * m_betao *
139	(13.0 - 78.0 * m_theta2 + 137.0 * theta4);
140
141	const double x1m5th = 1.0 - 5.0 * m_theta2;
142
143	m_omgdot = -0.5 * temp1 * x1m5th + 0.0625 * temp2 *
144	(7.0 - 114.0 * m_theta2 + 395.0 * theta4) +
145	temp3 * (3.0 - 36.0 * m_theta2 + 49.0 * theta4);
146
147	const double xhdot1 = -temp1 * m_cosio;
148
149	m_xnodot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * m_theta2) +
150	2.0 * temp3 * (3.0 - 7.0 * m_theta2)) * m_cosio;
151	m_xnodcf = 3.5 * m_betao2 * xhdot1 * m_c1;
152	m_t2cof = 1.5 * m_c1;
153	m_xlcof = 0.125 * a3ovk2 * m_sinio *
154	(3.0 + 5.0 * m_cosio) / (1.0 + m_cosio);
155	m_aycof = 0.25 * a3ovk2 * m_sinio;
156	m_x7thm1 = 7.0 * m_theta2 - 1.0;
157	}
158
159	//////////////////////////////////////////////////////////////////////////////
160	bool cNoradBase::FinalPosition(double incl, double omega,
161	double e, double a,
162	double xl, double xnode,
163	double xn, double tsince,
164	cEci &eci)
165	{
166	if ((e * e) > 1.0)
167	{
168	// error in satellite data
169	return false;
170	}
171
172	double beta = sqrt(1.0 - e * e);
173
174	// Long period periodics
175	double axn = e * cos(omega);
176	double temp = 1.0 / (a * beta * beta);
177	double xll = temp * m_xlcof * axn;
178	double aynl = temp * m_aycof;
179	double xlt = xl + xll;
180	double ayn = e * sin(omega) + aynl;
181
182	// Solve Kepler's Equation
183
184	double capu = Fmod2p(xlt - xnode);
185	double temp2 = capu;
186	double temp3 = 0.0;
187	double temp4 = 0.0;
188	double temp5 = 0.0;
189	double temp6 = 0.0;
190	double sinepw = 0.0;
191	double cosepw = 0.0;
192	bool fDone = false;
193
194	for (int i = 1; (i <= 10) && !fDone; i++)
195	{
196	sinepw = sin(temp2);
197	cosepw = cos(temp2);
198	temp3 = axn * sinepw;
199	temp4 = ayn * cosepw;
200	temp5 = axn * cosepw;
201	temp6 = ayn * sinepw;
202
203	double epw = (capu - temp4 + temp3 - temp2) /
204	(1.0 - temp5 - temp6) + temp2;
205
206	if (fabs(epw - temp2) <= E6A)
207	fDone = true;
208	else
209	temp2 = epw;
210	}
211
212	// Short period preliminary quantities
213	double ecose = temp5 + temp6;
214	double esine = temp3 - temp4;
215	double elsq = axn * axn + ayn * ayn;
216	temp = 1.0 - elsq;
217	double pl = a * temp;
218	double r = a * (1.0 - ecose);
219	double temp1 = 1.0 / r;
220	double rdot = XKE * sqrt(a) * esine * temp1;
221	double rfdot = XKE * sqrt(pl) * temp1;
222	temp2 = a * temp1;
223	double betal = sqrt(temp);
224	temp3 = 1.0 / (1.0 + betal);
225	double cosu = temp2 * (cosepw - axn + ayn * esine * temp3);
226	double sinu = temp2 * (sinepw - ayn - axn * esine * temp3);
227	double u = AcTan(sinu, cosu);
228	double sin2u = 2.0 * sinu * cosu;
229	double cos2u = 2.0 * cosu * cosu - 1.0;
230
231	temp = 1.0 / pl;
232	temp1 = CK2 * temp;
233	temp2 = temp1 * temp;
234
235	// Update for short periodics
236	double rk = r * (1.0 - 1.5 * temp2 * betal * m_x3thm1) +
237	0.5 * temp1 * m_x1mth2 * cos2u;
238	double uk = u - 0.25 * temp2 * m_x7thm1 * sin2u;
239	double xnodek = xnode + 1.5 * temp2 * m_cosio * sin2u;
240	double xinck = incl + 1.5 * temp2 * m_cosio * m_sinio * cos2u;
241	double rdotk = rdot - xn * temp1 * m_x1mth2 * sin2u;
242	double rfdotk = rfdot + xn * temp1 * (m_x1mth2 * cos2u + 1.5 * m_x3thm1);
243
244	// Orientation vectors
245	double sinuk = sin(uk);
246	double cosuk = cos(uk);
247	double sinik = sin(xinck);
248	double cosik = cos(xinck);
249	double sinnok = sin(xnodek);
250	double cosnok = cos(xnodek);
251	double xmx = -sinnok * cosik;
252	double xmy = cosnok * cosik;
253	double ux = xmx * sinuk + cosnok * cosuk;
254	double uy = xmy * sinuk + sinnok * cosuk;
255	double uz = sinik * sinuk;
256	double vx = xmx * cosuk - cosnok * sinuk;
257	double vy = xmy * cosuk - sinnok * sinuk;
258	double vz = sinik * cosuk;
259
260	// Position
261	double x = rk * ux;
262	double y = rk * uy;
263	double z = rk * uz;
264
265	cVector vecPos(x, y, z);
266
267	// Validate on altitude
268	double altKm = (vecPos.Magnitude() * (XKMPER_WGS72 / AE));
269
270	if ((altKm < XKMPER_WGS72) \|\| (altKm > (2 * GEOSYNC_ALT)))
271	return false;
272
273	// Velocity
274	double xdot = rdotk * ux + rfdotk * vx;
275	double ydot = rdotk * uy + rfdotk * vy;
276	double zdot = rdotk * uz + rfdotk * vz;
277
278	cVector vecVel(xdot, ydot, zdot);
279
280	cJulian gmt = m_Orbit.Epoch();
281	gmt.addMin(tsince);
282
283	eci = cEci(vecPos, vecVel, gmt);
284
285	return true;
286	}
287
288
289