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	kusanagi	1.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