TLEPropagator.java

  1. /* Copyright 2002-2016 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.propagation.analytical.tle;

  18. import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
  19. import org.apache.commons.math3.util.FastMath;
  20. import org.apache.commons.math3.util.MathUtils;
  21. import org.orekit.attitudes.Attitude;
  22. import org.orekit.attitudes.AttitudeProvider;
  23. import org.orekit.errors.OrekitException;
  24. import org.orekit.errors.OrekitMessages;
  25. import org.orekit.errors.PropagationException;
  26. import org.orekit.frames.Frame;
  27. import org.orekit.frames.FramesFactory;
  28. import org.orekit.orbits.CartesianOrbit;
  29. import org.orekit.orbits.Orbit;
  30. import org.orekit.propagation.SpacecraftState;
  31. import org.orekit.propagation.analytical.AbstractAnalyticalPropagator;
  32. import org.orekit.time.AbsoluteDate;
  33. import org.orekit.utils.PVCoordinates;


  34. /** This class provides elements to propagate TLE's.
  35.  * <p>
  36.  * The models used are SGP4 and SDP4, initially proposed by NORAD as the unique convenient
  37.  * propagator for TLE's. Inputs and outputs of this propagator are only suited for
  38.  * NORAD two lines elements sets, since it uses estimations and mean values appropriate
  39.  * for TLE's only.
  40.  * </p>
  41.  * <p>
  42.  * Deep- or near- space propagator is selected internally according to NORAD recommendations
  43.  * so that the user has not to worry about the used computation methods. One instance is created
  44.  * for each TLE (this instance can only be get using {@link #selectExtrapolator(TLE)} method,
  45.  * and can compute {@link PVCoordinates position and velocity coordinates} at any
  46.  * time. Maximum accuracy is guaranteed in a 24h range period before and after the provided
  47.  * TLE epoch (of course this accuracy is not really measurable nor predictable: according to
  48.  * <a href="http://www.celestrak.com/">CelesTrak</a>, the precision is close to one kilometer
  49.  * and error won't probably rise above 2 km).
  50.  * </p>
  51.  * <p>This implementation is largely inspired from the paper and source code <a
  52.  * href="http://www.celestrak.com/publications/AIAA/2006-6753/">Revisiting Spacetrack
  53.  * Report #3</a> and is fully compliant with its results and tests cases.</p>
  54.  * @author Felix R. Hoots, Ronald L. Roehrich, December 1980 (original fortran)
  55.  * @author David A. Vallado, Paul Crawford, Richard Hujsak, T.S. Kelso (C++ translation and improvements)
  56.  * @author Fabien Maussion (java translation)
  57.  * @see TLE
  58.  */
  59. public abstract class TLEPropagator extends AbstractAnalyticalPropagator {

  60.     // CHECKSTYLE: stop VisibilityModifierCheck

  61.     /** Initial state. */
  62.     protected final TLE tle;

  63.     /** final RAAN. */
  64.     protected double xnode;

  65.     /** final semi major axis. */
  66.     protected double a;

  67.     /** final eccentricity. */
  68.     protected double e;

  69.     /** final inclination. */
  70.     protected double i;

  71.     /** final perigee argument. */
  72.     protected double omega;

  73.     /** L from SPTRCK #3. */
  74.     protected double xl;

  75.     /** original recovered semi major axis. */
  76.     protected double a0dp;

  77.     /** original recovered mean motion. */
  78.     protected double xn0dp;

  79.     /** cosinus original inclination. */
  80.     protected double cosi0;

  81.     /** cos io squared. */
  82.     protected double theta2;

  83.     /** sinus original inclination. */
  84.     protected double sini0;

  85.     /** common parameter for mean anomaly (M) computation. */
  86.     protected double xmdot;

  87.     /** common parameter for perigee argument (omega) computation. */
  88.     protected double omgdot;

  89.     /** common parameter for raan (OMEGA) computation. */
  90.     protected double xnodot;

  91.     /** original eccentricity squared. */
  92.     protected double e0sq;
  93.     /** 1 - e2. */
  94.     protected double beta02;

  95.     /** sqrt (1 - e2). */
  96.     protected double beta0;

  97.     /** perigee, expressed in KM and ALTITUDE. */
  98.     protected double perige;

  99.     /** eta squared. */
  100.     protected double etasq;

  101.     /** original eccentricity * eta. */
  102.     protected double eeta;

  103.     /** s* new value for the contant s. */
  104.     protected double s4;

  105.     /** tsi from SPTRCK #3. */
  106.     protected double tsi;

  107.     /** eta from SPTRCK #3. */
  108.     protected double eta;

  109.     /** coef for SGP C3 computation. */
  110.     protected double coef;

  111.     /** coef for SGP C5 computation. */
  112.     protected double coef1;

  113.     /** C1 from SPTRCK #3. */
  114.     protected double c1;

  115.     /** C2 from SPTRCK #3. */
  116.     protected double c2;

  117.     /** C4 from SPTRCK #3. */
  118.     protected double c4;

  119.     /** common parameter for raan (OMEGA) computation. */
  120.     protected double xnodcf;

  121.     /** 3/2 * C1. */
  122.     protected double t2cof;

  123.     // CHECKSTYLE: resume VisibilityModifierCheck

  124.     /** TLE frame. */
  125.     private final Frame teme;

  126.     /** Spacecraft mass (kg). */
  127.     private final double mass;

  128.     /** Protected constructor for derived classes.
  129.      * @param initialTLE the unique TLE to propagate
  130.      * @param attitudeProvider provider for attitude computation
  131.      * @param mass spacecraft mass (kg)
  132.      * @exception OrekitException if some specific error occurs
  133.      */
  134.     protected TLEPropagator(final TLE initialTLE, final AttitudeProvider attitudeProvider,
  135.                             final double mass)
  136.         throws OrekitException {
  137.         super(attitudeProvider);
  138.         setStartDate(initialTLE.getDate());
  139.         this.tle  = initialTLE;
  140.         this.teme = FramesFactory.getTEME();
  141.         this.mass = mass;
  142.         initializeCommons();
  143.         sxpInitialize();
  144.         // set the initial state
  145.         final Orbit orbit = propagateOrbit(initialTLE.getDate());
  146.         final Attitude attitude = attitudeProvider.getAttitude(orbit, orbit.getDate(), orbit.getFrame());
  147.         super.resetInitialState(new SpacecraftState(orbit, attitude, mass));
  148.     }

  149.     /** Selects the extrapolator to use with the selected TLE.
  150.      * @param tle the TLE to propagate.
  151.      * @return the correct propagator.
  152.      * @exception OrekitException if the underlying model cannot be initialized
  153.      */
  154.     public static TLEPropagator selectExtrapolator(final TLE tle) throws OrekitException {
  155.         return selectExtrapolator(tle, DEFAULT_LAW, DEFAULT_MASS);
  156.     }

  157.     /** Selects the extrapolator to use with the selected TLE.
  158.      * @param tle the TLE to propagate.
  159.      * @param attitudeProvider provider for attitude computation
  160.      * @param mass spacecraft mass (kg)
  161.      * @return the correct propagator.
  162.      * @exception OrekitException if the underlying model cannot be initialized
  163.      */
  164.     public static TLEPropagator selectExtrapolator(final TLE tle, final AttitudeProvider attitudeProvider,
  165.                                                    final double mass) throws OrekitException {

  166.         final double a1 = FastMath.pow( TLEConstants.XKE / (tle.getMeanMotion() * 60.0), TLEConstants.TWO_THIRD);
  167.         final double cosi0 = FastMath.cos(tle.getI());
  168.         final double temp = TLEConstants.CK2 * 1.5 * (3 * cosi0 * cosi0 - 1.0) *
  169.                             FastMath.pow(1.0 - tle.getE() * tle.getE(), -1.5);
  170.         final double delta1 = temp / (a1 * a1);
  171.         final double a0 = a1 * (1.0 - delta1 * (TLEConstants.ONE_THIRD + delta1 * (delta1 * 134.0 / 81.0 + 1.0)));
  172.         final double delta0 = temp / (a0 * a0);

  173.         // recover original mean motion :
  174.         final double xn0dp = tle.getMeanMotion() * 60.0 / (delta0 + 1.0);

  175.         // Period >= 225 minutes is deep space
  176.         if (MathUtils.TWO_PI / (xn0dp * TLEConstants.MINUTES_PER_DAY) >= (1.0 / 6.4)) {
  177.             return new DeepSDP4(tle, attitudeProvider, mass);
  178.         } else {
  179.             return new SGP4(tle, attitudeProvider, mass);
  180.         }
  181.     }

  182.     /** Get the Earth gravity coefficient used for TLE propagation.
  183.      * @return the Earth gravity coefficient.
  184.      */
  185.     public static double getMU() {
  186.         return TLEConstants.MU;
  187.     }

  188.     /** Get the extrapolated position and velocity from an initial TLE.
  189.      * @param date the final date
  190.      * @return the final PVCoordinates
  191.      * @exception OrekitException if propagation cannot be performed at given date
  192.      */
  193.     public PVCoordinates getPVCoordinates(final AbsoluteDate date)
  194.         throws OrekitException {

  195.         sxpPropagate(date.durationFrom(tle.getDate()) / 60.0);

  196.         // Compute PV with previous calculated parameters
  197.         return computePVCoordinates();
  198.     }

  199.     /** Computation of the first commons parameters.
  200.      */
  201.     private void initializeCommons() {

  202.         final double a1 = FastMath.pow(TLEConstants.XKE / (tle.getMeanMotion() * 60.0), TLEConstants.TWO_THIRD);
  203.         cosi0 = FastMath.cos(tle.getI());
  204.         theta2 = cosi0 * cosi0;
  205.         final double x3thm1 = 3.0 * theta2 - 1.0;
  206.         e0sq = tle.getE() * tle.getE();
  207.         beta02 = 1.0 - e0sq;
  208.         beta0 = FastMath.sqrt(beta02);
  209.         final double tval = TLEConstants.CK2 * 1.5 * x3thm1 / (beta0 * beta02);
  210.         final double delta1 = tval / (a1 * a1);
  211.         final double a0 = a1 * (1.0 - delta1 * (TLEConstants.ONE_THIRD + delta1 * (1.0 + 134.0 / 81.0 * delta1)));
  212.         final double delta0 = tval / (a0 * a0);

  213.         // recover original mean motion and semi-major axis :
  214.         xn0dp = tle.getMeanMotion() * 60.0 / (delta0 + 1.0);
  215.         a0dp = a0 / (1.0 - delta0);

  216.         // Values of s and qms2t :
  217.         s4 = TLEConstants.S;  // unmodified value for s
  218.         double q0ms24 = TLEConstants.QOMS2T; // unmodified value for q0ms2T

  219.         perige = (a0dp * (1 - tle.getE()) - TLEConstants.NORMALIZED_EQUATORIAL_RADIUS) * TLEConstants.EARTH_RADIUS; // perige

  220.         //  For perigee below 156 km, the values of s and qoms2t are changed :
  221.         if (perige < 156.0) {
  222.             if (perige <= 98.0) {
  223.                 s4 = 20.0;
  224.             } else {
  225.                 s4 = perige - 78.0;
  226.             }
  227.             final double temp_val = (120.0 - s4) * TLEConstants.NORMALIZED_EQUATORIAL_RADIUS / TLEConstants.EARTH_RADIUS;
  228.             final double temp_val_squared = temp_val * temp_val;
  229.             q0ms24 = temp_val_squared * temp_val_squared;
  230.             s4 = s4 / TLEConstants.EARTH_RADIUS + TLEConstants.NORMALIZED_EQUATORIAL_RADIUS; // new value for q0ms2T and s
  231.         }

  232.         final double pinv = 1.0 / (a0dp * beta02);
  233.         final double pinvsq = pinv * pinv;
  234.         tsi = 1.0 / (a0dp - s4);
  235.         eta = a0dp * tle.getE() * tsi;
  236.         etasq = eta * eta;
  237.         eeta = tle.getE() * eta;

  238.         final double psisq = FastMath.abs(1.0 - etasq); // abs because pow 3.5 needs positive value
  239.         final double tsi_squared = tsi * tsi;
  240.         coef = q0ms24 * tsi_squared * tsi_squared;
  241.         coef1 = coef / FastMath.pow(psisq, 3.5);

  242.         // C2 and C1 coefficients computation :
  243.         c2 = coef1 * xn0dp * (a0dp * (1.0 + 1.5 * etasq + eeta * (4.0 + etasq)) +
  244.              0.75 * TLEConstants.CK2 * tsi / psisq * x3thm1 * (8.0 + 3.0 * etasq * (8.0 + etasq)));
  245.         c1 = tle.getBStar() * c2;
  246.         sini0 = FastMath.sin(tle.getI());

  247.         final double x1mth2 = 1.0 - theta2;

  248.         // C4 coefficient computation :
  249.         c4 = 2.0 * xn0dp * coef1 * a0dp * beta02 * (eta * (2.0 + 0.5 * etasq) +
  250.              tle.getE() * (0.5 + 2.0 * etasq) -
  251.              2 * TLEConstants.CK2 * tsi / (a0dp * psisq) *
  252.              (-3.0 * x3thm1 * (1.0 - 2.0 * eeta + etasq * (1.5 - 0.5 * eeta)) +
  253.               0.75 * x1mth2 * (2.0 * etasq - eeta * (1.0 + etasq)) * FastMath.cos(2.0 * tle.getPerigeeArgument())));

  254.         final double theta4 = theta2 * theta2;
  255.         final double temp1 = 3 * TLEConstants.CK2 * pinvsq * xn0dp;
  256.         final double temp2 = temp1 * TLEConstants.CK2 * pinvsq;
  257.         final double temp3 = 1.25 * TLEConstants.CK4 * pinvsq * pinvsq * xn0dp;

  258.         // atmospheric and gravitation coefs :(Mdf and OMEGAdf)
  259.         xmdot = xn0dp +
  260.                 0.5 * temp1 * beta0 * x3thm1 +
  261.                 0.0625 * temp2 * beta0 * (13.0 - 78.0 * theta2 + 137.0 * theta4);

  262.         final double x1m5th = 1.0 - 5.0 * theta2;

  263.         omgdot = -0.5 * temp1 * x1m5th +
  264.                  0.0625 * temp2 * (7.0 - 114.0 * theta2 + 395.0 * theta4) +
  265.                  temp3 * (3.0 - 36.0 * theta2 + 49.0 * theta4);

  266.         final double xhdot1 = -temp1 * cosi0;

  267.         xnodot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * theta2) + 2.0 * temp3 * (3.0 - 7.0 * theta2)) * cosi0;
  268.         xnodcf = 3.5 * beta02 * xhdot1 * c1;
  269.         t2cof = 1.5 * c1;

  270.     }

  271.     /** Retrieves the position and velocity.
  272.      * @return the computed PVCoordinates.
  273.      * @exception OrekitException if current orbit is out of supported range
  274.      * (too large eccentricity, too low perigee ...)
  275.      */
  276.     private PVCoordinates computePVCoordinates() throws OrekitException {

  277.         // Long period periodics
  278.         final double axn = e * FastMath.cos(omega);
  279.         double temp = 1.0 / (a * (1.0 - e * e));
  280.         final double xlcof = 0.125 * TLEConstants.A3OVK2 * sini0 * (3.0 + 5.0 * cosi0) / (1.0 + cosi0);
  281.         final double aycof = 0.25 * TLEConstants.A3OVK2 * sini0;
  282.         final double xll = temp * xlcof * axn;
  283.         final double aynl = temp * aycof;
  284.         final double xlt = xl + xll;
  285.         final double ayn = e * FastMath.sin(omega) + aynl;
  286.         final double elsq = axn * axn + ayn * ayn;
  287.         final double capu = MathUtils.normalizeAngle(xlt - xnode, FastMath.PI);
  288.         double epw = capu;
  289.         double ecosE = 0;
  290.         double esinE = 0;
  291.         double sinEPW = 0;
  292.         double cosEPW = 0;

  293.         // Dundee changes:  items dependent on cosio get recomputed:
  294.         final double cosi0Sq = cosi0 * cosi0;
  295.         final double x3thm1 = 3.0 * cosi0Sq - 1.0;
  296.         final double x1mth2 = 1.0 - cosi0Sq;
  297.         final double x7thm1 = 7.0 * cosi0Sq - 1.0;

  298.         if (e > (1 - 1e-6)) {
  299.             throw new OrekitException(OrekitMessages.TOO_LARGE_ECCENTRICITY_FOR_PROPAGATION_MODEL, e);
  300.         }

  301.         // Solve Kepler's' Equation.
  302.         final double newtonRaphsonEpsilon = 1e-12;
  303.         for (int j = 0; j < 10; j++) {

  304.             boolean doSecondOrderNewtonRaphson = true;

  305.             sinEPW = FastMath.sin( epw);
  306.             cosEPW = FastMath.cos( epw);
  307.             ecosE = axn * cosEPW + ayn * sinEPW;
  308.             esinE = axn * sinEPW - ayn * cosEPW;
  309.             final double f = capu - epw + esinE;
  310.             if (FastMath.abs(f) < newtonRaphsonEpsilon) {
  311.                 break;
  312.             }
  313.             final double fdot = 1.0 - ecosE;
  314.             double delta_epw = f / fdot;
  315.             if (j == 0) {
  316.                 final double maxNewtonRaphson = 1.25 * FastMath.abs(e);
  317.                 doSecondOrderNewtonRaphson = false;
  318.                 if (delta_epw > maxNewtonRaphson) {
  319.                     delta_epw = maxNewtonRaphson;
  320.                 } else if (delta_epw < -maxNewtonRaphson) {
  321.                     delta_epw = -maxNewtonRaphson;
  322.                 } else {
  323.                     doSecondOrderNewtonRaphson = true;
  324.                 }
  325.             }
  326.             if (doSecondOrderNewtonRaphson) {
  327.                 delta_epw = f / (fdot + 0.5 * esinE * delta_epw);
  328.             }
  329.             epw += delta_epw;
  330.         }

  331.         // Short period preliminary quantities
  332.         temp = 1.0 - elsq;
  333.         final double pl = a * temp;
  334.         final double r = a * (1.0 - ecosE);
  335.         double temp2 = a / r;
  336.         final double betal = FastMath.sqrt(temp);
  337.         temp = esinE / (1.0 + betal);
  338.         final double cosu = temp2 * (cosEPW - axn + ayn * temp);
  339.         final double sinu = temp2 * (sinEPW - ayn - axn * temp);
  340.         final double u = FastMath.atan2(sinu, cosu);
  341.         final double sin2u = 2.0 * sinu * cosu;
  342.         final double cos2u = 2.0 * cosu * cosu - 1.0;
  343.         final double temp1 = TLEConstants.CK2 / pl;
  344.         temp2 = temp1 / pl;

  345.         // Update for short periodics
  346.         final double rk = r * (1.0 - 1.5 * temp2 * betal * x3thm1) + 0.5 * temp1 * x1mth2 * cos2u;
  347.         final double uk = u - 0.25 * temp2 * x7thm1 * sin2u;
  348.         final double xnodek = xnode + 1.5 * temp2 * cosi0 * sin2u;
  349.         final double xinck = i + 1.5 * temp2 * cosi0 * sini0 * cos2u;

  350.         // Orientation vectors
  351.         final double sinuk = FastMath.sin(uk);
  352.         final double cosuk = FastMath.cos(uk);
  353.         final double sinik = FastMath.sin(xinck);
  354.         final double cosik = FastMath.cos(xinck);
  355.         final double sinnok = FastMath.sin(xnodek);
  356.         final double cosnok = FastMath.cos(xnodek);
  357.         final double xmx = -sinnok * cosik;
  358.         final double xmy = cosnok * cosik;
  359.         final double ux = xmx * sinuk + cosnok * cosuk;
  360.         final double uy = xmy * sinuk + sinnok * cosuk;
  361.         final double uz = sinik * sinuk;

  362.         // Position and velocity
  363.         final double cr = 1000 * rk * TLEConstants.EARTH_RADIUS;
  364.         final Vector3D pos = new Vector3D(cr * ux, cr * uy, cr * uz);

  365.         final double rdot   = TLEConstants.XKE * FastMath.sqrt(a) * esinE / r;
  366.         final double rfdot  = TLEConstants.XKE * FastMath.sqrt(pl) / r;
  367.         final double xn     = TLEConstants.XKE / (a * FastMath.sqrt(a));
  368.         final double rdotk  = rdot - xn * temp1 * x1mth2 * sin2u;
  369.         final double rfdotk = rfdot + xn * temp1 * (x1mth2 * cos2u + 1.5 * x3thm1);
  370.         final double vx     = xmx * cosuk - cosnok * sinuk;
  371.         final double vy     = xmy * cosuk - sinnok * sinuk;
  372.         final double vz     = sinik * cosuk;

  373.         final double cv = 1000.0 * TLEConstants.EARTH_RADIUS / 60.0;
  374.         final Vector3D vel = new Vector3D(cv * (rdotk * ux + rfdotk * vx),
  375.                                           cv * (rdotk * uy + rfdotk * vy),
  376.                                           cv * (rdotk * uz + rfdotk * vz));

  377.         return new PVCoordinates(pos, vel);

  378.     }

  379.     /** Initialization proper to each propagator (SGP or SDP).
  380.      * @exception OrekitException if some specific error occurs
  381.      */
  382.     protected abstract void sxpInitialize() throws OrekitException;

  383.     /** Propagation proper to each propagator (SGP or SDP).
  384.      * @param t the offset from initial epoch (min)
  385.      * @exception OrekitException if current state cannot be propagated
  386.      */
  387.     protected abstract void sxpPropagate(double t) throws OrekitException;

  388.     /** {@inheritDoc} */
  389.     public void resetInitialState(final SpacecraftState state)
  390.         throws PropagationException {
  391.         throw new PropagationException(OrekitMessages.NON_RESETABLE_STATE);
  392.     }

  393.     /** {@inheritDoc} */
  394.     protected void resetIntermediateState(final SpacecraftState state, final boolean forward)
  395.         throws PropagationException {
  396.         throw new PropagationException(OrekitMessages.NON_RESETABLE_STATE);
  397.     }

  398.     /** {@inheritDoc} */
  399.     protected double getMass(final AbsoluteDate date) {
  400.         return mass;
  401.     }

  402.     /** {@inheritDoc} */
  403.     protected Orbit propagateOrbit(final AbsoluteDate date) throws PropagationException {
  404.         try {
  405.             return new CartesianOrbit(getPVCoordinates(date), teme, date, TLEConstants.MU);
  406.         } catch (OrekitException oe) {
  407.             throw new PropagationException(oe);
  408.         }
  409.     }

  410.     /** Get the underlying TLE.
  411.      * @return underlying TLE
  412.      */
  413.     public TLE getTLE() {
  414.         return tle;
  415.     }

  416.     /** {@inheritDoc} */
  417.     public Frame getFrame() {
  418.         return teme;
  419.     }

  420. }