Changes to FindPositionSDP4()

SGDP4.cpp

@@ -135,7 +135,7 @@ void SGDP4::Initialize(const double& theta2, const double& betao2, const double&
     double qoms24 = constants_.QOMS2T;
     if (Perigee() < 156.0) {
         s4 = Perigee() - 78.0;
-        if (Perigee() <= 98.0) {
+        if (Perigee() < 98.0) {
             s4 = 20.0;
         }
         qoms24 = pow((120.0 - s4) * constants_.AE / constants_.XKMPER, 4.0);
@@ -256,12 +256,12 @@ void SGDP4::FindPositionSDP4(Eci& eci, double tsince) {
     /*
      * the final values
      */
-    double e;
+    double e = Eccentricity();
     double a;
     double omega;
     double xl;
     double xnode;
-    double xincl;
+    double xincl = Inclination();
 
     /*
      * update for secular gravity and atmospheric drag
@@ -270,33 +270,58 @@ void SGDP4::FindPositionSDP4(Eci& eci, double tsince) {
     double omgadf = ArgumentPerigee() + i_omgdot_ * tsince;
     const double xnoddf = AscendingNode() + i_xnodot_ * tsince;
 
+    omega = omgadf;
+
     const double tsq = tsince * tsince;
     xnode = xnoddf + i_xnodcf_ * tsq;
     double tempa = 1.0 - i_c1_ * tsince;
     double tempe = BStar() * i_c4_ * tsince;
     double templ = i_t2cof_ * tsq;
 
-    double em = Eccentricity();
-    double xinc = Inclination();
     double xn = RecoveredMeanMotion();
 
-    DeepSecular(tsince, xmdf, omgadf, xnode, em, xinc, xn);
+    /* t, xll, omgasm, xnodes, em, xinc, xn */
+    DeepSecular(tsince, xmdf, omega, xnode, e, xincl, xn);
 
-    a = pow(constants_.XKE / xn, constants_.TWOTHRD) * pow(tempa, 2.0);
+    if (xn <= 0.0) {
-    e = em - tempe;
+        throw new SatelliteException("Error: 2 (xn <= 0.0)");
-    double xmam = xmdf + RecoveredMeanMotion() * templ;
-
-    DeepPeriodics(tsince, e, xinc, omgadf, xnode, xmam);
-
-    if (xinc < 0.0) {
-        xinc = -xinc;
-        xnode += Globals::PI();
-        omgadf -= Globals::PI();
     }
 
-    xl = xmam + omgadf + xnode;
+    a = pow(constants_.XKE / xn, constants_.TWOTHRD) * pow(tempa, 2.0);
-    xincl = xinc;
+
-    omega = omgadf;
+    e -= tempe;
+    /*
+     * fix tolerance for error recognition
+     */
+    if (e >= 1.0 || e < -0.001) {
+        throw new SatelliteException("Error: 1 (e >= 1.0 || e < -0.001)");
+    }
+    /*
+     * fix tolerance to avoid a divide by zero
+     */
+    if (e < 1.0e-6)
+        e = 1.0e-6;
+
+    xmdf += RecoveredMeanMotion() * templ;
+    double xlm = xmdf + omega + xnode;
+    xnode = fmod(xnode, Globals::TWOPI());
+    omega = fmod(omega, Globals::TWOPI());
+    xlm = fmod(xlm, Globals::TWOPI());
+    double xmam = fmod(xlm - omega - xnode, Globals::TWOPI());
+
+    DeepPeriodics(tsince, e, xincl, omega, xnode, xmam);
+
+    if (xincl < 0.0) {
+        xincl = -xincl;
+        xnode += Globals::PI();
+        omega -= Globals::PI();
+    }
+
+    xl = xmam + omega + xnode;
+
+    if (e < 0.0 || e > 1.0) {
+        throw new SatelliteException("Error: 3 (e < 0.0 || e > 1.0)");
+    }
 
     /*
      * re-compute the perturbed values
@@ -599,7 +624,7 @@ void SGDP4::DeepSpaceInitialize(const double& eosq, const double& sinio, const d
      */
     const double d_day_ = Epoch().FromJan1_12h_1900();
 
-    const double xnodce = 4.5236020 - 9.2422029e-4 * d_day_;
+    const double xnodce = fmod(4.5236020 - 9.2422029e-4 * d_day_, Globals::TWOPI());
     const double stem = sin(xnodce);
     const double ctem = cos(xnodce);
     const double zcosil = 0.91375164 - 0.03568096 * ctem;
@@ -1007,7 +1032,7 @@ void SGDP4::DeepPeriodics(const double& t, double& em,
 void SGDP4::DeepSecular(const double& t, double& xll, double& omgasm,
         double& xnodes, double& em, double& xinc, double& xn) {
 
-    static const double THDT = 4.3752691E-3;
+    static const double THDT = 4.37526908801129966e-3;
 
     static const double STEP2 = 259200.0;
     static const double STEP = 720.0;

SGDP4.h

@@ -45,20 +45,20 @@ private:
      */
     double i_cosio_;
     double i_sinio_;
-    double i_x3thm1_;
     double i_eta_;
-    double i_c1_;
+    double i_t2cof_;
     double i_a3ovk2_;
     double i_x1mth2_;
-    double i_c4_;
+    double i_x3thm1_;
-    double i_xmdot_;
-    double i_omgdot_;
-    double i_xnodot_;
-    double i_xnodcf_;
-    double i_t2cof_;
-    double i_xlcof_;
-    double i_aycof_;
     double i_x7thm1_;
+    double i_aycof_;
+    double i_xlcof_;
+    double i_xnodcf_;
+    double i_c1_;
+    double i_c4_;
+    double i_omgdot_; // secular rate of omega (radians/sec)
+    double i_xnodot_; // secular rate of xnode (radians/sec)
+    double i_xmdot_; // secular rate of xmo (radians/sec)
     /*
      * sgp4 constant
      */

main.cpp

@@ -144,6 +144,8 @@ void RunTle(Tle tle, double start, double end, double inc) {
 
 void RunTest() {
 
+#if 0
+
     /*
       #   ------------------ Verification test cases ----------------------
       #                       # TEME example
@@ -379,7 +381,7 @@ void RunTest() {
     RunTle(Tle("28350",
             "1 28350U 04020A   06167.21788666  .16154492  76267-5  18678-3 0  8894",
             "2 28350  64.9977 345.6130 0024870 260.7578  99.9590 16.47856722116490"), 0.0, 2880.0, 120.0);
-
+#endif
 
     /*
     #   H-2 R/B                   # Deep space, perigee = 135.75 (<156) s4 mod
@@ -390,7 +392,7 @@ void RunTest() {
             "1 28623U 05006B   06177.81079184  .00637644  69054-6  96390-3 0  6000",
             "2 28623  28.5200 114.9834 6249053 170.2550 212.8965  3.79477162 12753"), 0.0, 1440.0, 120.0);
 
-
+    return;
     /*
     #   XM-3                      # 24h resonant geo, incl < 3 deg goes
     #                       # negative around 1130 min