FieldTLEPropagator.java

  1. /* Copyright 2002-2025 CS GROUP
  2.  * Licensed to CS GROUP (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;


  20. import java.util.List;

  22. import org.hipparchus.CalculusFieldElement;
  23. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  24. import org.hipparchus.util.FastMath;
  25. import org.hipparchus.util.MathUtils;
  26. import org.hipparchus.util.Pair;
  27. import org.orekit.annotation.DefaultDataContext;
  28. import org.orekit.attitudes.AttitudeProvider;
  29. import org.orekit.attitudes.FieldAttitude;
  30. import org.orekit.attitudes.FrameAlignedProvider;
  31. import org.orekit.data.DataContext;
  32. import org.orekit.errors.OrekitException;
  33. import org.orekit.errors.OrekitMessages;
  34. import org.orekit.frames.Frame;
  35. import org.orekit.orbits.FieldCartesianOrbit;
  36. import org.orekit.orbits.FieldOrbit;
  37. import org.orekit.propagation.FieldSpacecraftState;
  38. import org.orekit.propagation.analytical.FieldAbstractAnalyticalPropagator;
  39. import org.orekit.propagation.analytical.tle.generation.TleGenerationAlgorithm;
  40. import org.orekit.time.FieldAbsoluteDate;
  41. import org.orekit.time.TimeScale;
  42. import org.orekit.utils.FieldPVCoordinates;
  43. import org.orekit.utils.PVCoordinates;
  44. import org.orekit.utils.ParameterDriver;
  45. import org.orekit.utils.TimeSpanMap;


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

  78.     // CHECKSTYLE: stop VisibilityModifier check

  80.     /** Initial state. */
  81.     protected FieldTLE<T> tle;

  83.     /** UTC time scale. */
  84.     protected final TimeScale utc;

  86.     /** final RAAN. */
  87.     protected T xnode;

  89.     /** final semi major axis. */
  90.     protected T a;

  92.     /** final eccentricity. */
  93.     protected T e;

  95.     /** final inclination. */
  96.     protected T i;

  98.     /** final perigee argument. */
  99.     protected T omega;

  101.     /** L from SPTRCK #3. */
  102.     protected T xl;

  104.     /** original recovered semi major axis. */
  105.     protected T a0dp;

  107.     /** original recovered mean motion. */
  108.     protected T xn0dp;

  110.     /** cosinus original inclination. */
  111.     protected T cosi0;

  113.     /** cos io squared. */
  114.     protected T theta2;

  116.     /** sinus original inclination. */
  117.     protected T sini0;

  119.     /** common parameter for mean anomaly (M) computation. */
  120.     protected T xmdot;

  122.     /** common parameter for perigee argument (omega) computation. */
  123.     protected T omgdot;

  125.     /** common parameter for raan (OMEGA) computation. */
  126.     protected T xnodot;

  128.     /** original eccentricity squared. */
  129.     protected T e0sq;
  130.     /** 1 - e2. */
  131.     protected T beta02;

  133.     /** sqrt (1 - e2). */
  134.     protected T beta0;

  136.     /** perigee, expressed in KM and ALTITUDE. */
  137.     protected T perige;

  139.     /** eta squared. */
  140.     protected T etasq;

  142.     /** original eccentricity * eta. */
  143.     protected T eeta;

  145.     /** s* new value for the contant s. */
  146.     protected T s4;

  148.     /** tsi from SPTRCK #3. */
  149.     protected T tsi;

  151.     /** eta from SPTRCK #3. */
  152.     protected T eta;

  154.     /** coef for SGP C3 computation. */
  155.     protected T coef;

  157.     /** coef for SGP C5 computation. */
  158.     protected T coef1;

  160.     /** C1 from SPTRCK #3. */
  161.     protected T c1;

  163.     /** C2 from SPTRCK #3. */
  164.     protected T c2;

  166.     /** C4 from SPTRCK #3. */
  167.     protected T c4;

  169.     /** common parameter for raan (OMEGA) computation. */
  170.     protected T xnodcf;

  172.     /** 3/2 * C1. */
  173.     protected T t2cof;

  175.     // CHECKSTYLE: resume VisibilityModifier check

  177.     /** TLE frame. */
  178.     private final Frame teme;

  180.     /** All TLEs and masses. */
  181.     private TimeSpanMap<Pair<FieldTLE<T>, T>> tlesAndMasses;

  183.     /** Protected constructor for derived classes.
  184.      *
  185.      * <p>This constructor uses the {@link DataContext#getDefault() default data context}.
  186.      *
  187.      * @param initialTLE the unique TLE to propagate
  188.      * @param attitudeProvider provider for attitude computation
  189.      * @param mass spacecraft mass (kg)
  190.      * @param parameters SGP4 and SDP4 model parameters
  191.      * @see #FieldTLEPropagator(FieldTLE, AttitudeProvider, CalculusFieldElement, Frame, CalculusFieldElement[])
  192.      */
  193.     @DefaultDataContext
  194.     protected FieldTLEPropagator(final FieldTLE<T> initialTLE, final AttitudeProvider attitudeProvider, final T mass,
  195.                                  final T[] parameters) {
  196.         this(initialTLE, attitudeProvider, mass, DataContext.getDefault().getFrames().getTEME(), parameters);
  197.     }

  199.     /** Protected constructor for derived classes.
  200.      * @param initialTLE the unique TLE to propagate
  201.      * @param attitudeProvider provider for attitude computation
  202.      * @param mass spacecraft mass (kg)
  203.      * @param teme the TEME frame to use for propagation.
  204.      * @param parameters SGP4 and SDP4 model parameters
  205.      */
  206.     protected FieldTLEPropagator(final FieldTLE<T> initialTLE, final AttitudeProvider attitudeProvider, final T mass,
  207.                                  final Frame teme, final T[] parameters) {
  208.         super(initialTLE.getE().getField(), attitudeProvider);
  209.         setStartDate(initialTLE.getDate());
  210.         this.utc       = initialTLE.getUtc();
  211.         initializeTle(initialTLE);
  212.         this.teme      = teme;
  213.         this.tlesAndMasses      = new TimeSpanMap<>(new Pair<>(tle, mass));

  215.         initializeCommons(parameters);
  216.         sxpInitialize(parameters);
  217.         // set the initial state
  218.         final FieldOrbit<T> orbit = propagateOrbit(initialTLE.getDate(), parameters);
  219.         final FieldAttitude<T> attitude = attitudeProvider.getAttitude(orbit, orbit.getDate(), orbit.getFrame());
  220.         super.resetInitialState(new FieldSpacecraftState<>(orbit, attitude).withMass(mass));
  221.     }

  223.     /** Selects the extrapolator to use with the selected TLE.
  224.      *
  225.      * <p>This method uses the {@link DataContext#getDefault() default data context}.
  226.      *
  227.      * @param tle the TLE to propagate.
  228.      * @param parameters SGP4 and SDP4 model parameters
  229.      * @return the correct propagator.
  230.      * @param <T> elements type
  231.      * @see #selectExtrapolator(FieldTLE, Frame, CalculusFieldElement[])
  232.      */
  233.     @DefaultDataContext
  234.     public static <T extends CalculusFieldElement<T>> FieldTLEPropagator<T> selectExtrapolator(final FieldTLE<T> tle, final T[] parameters) {
  235.         return selectExtrapolator(tle, DataContext.getDefault().getFrames().getTEME(), parameters);
  236.     }

  238.     /** Selects the extrapolator to use with the selected TLE.
  239.      *
  240.      *<p>This method uses the {@link DataContext#getDefault() default data context}.
  241.      *
  242.      * @param tle the TLE to propagate.
  243.      * @param teme TEME frame.
  244.      * @param parameters SGP4 and SDP4 model parameters
  245.      * @return the correct propagator.
  246.      * @param <T> elements type
  247.      */
  248.     public static <T extends CalculusFieldElement<T>> FieldTLEPropagator<T> selectExtrapolator(final FieldTLE<T> tle, final Frame teme, final T[] parameters) {
  249.         return selectExtrapolator(
  250.                 tle,
  251.                 FrameAlignedProvider.of(teme),
  252.                 tle.getE().getField().getZero().newInstance(DEFAULT_MASS),
  253.                 teme,
  254.                 parameters);
  255.     }

  257.     /** Selects the extrapolator to use with the selected TLE.
  258.      *
  259.      * <p>This method uses the {@link DataContext#getDefault() default data context}.
  260.      *
  261.      * @param tle the TLE to propagate.
  262.      * @param attitudeProvider provider for attitude computation
  263.      * @param mass spacecraft mass (kg)
  264.      * @param parameters SGP4 and SDP4 model parameters
  265.      * @return the correct propagator.
  266.      * @param <T> elements type
  267.      * @see #selectExtrapolator(FieldTLE, AttitudeProvider, CalculusFieldElement, Frame, CalculusFieldElement[])
  268.      */
  269.     @DefaultDataContext
  270.     public static <T extends CalculusFieldElement<T>> FieldTLEPropagator<T> selectExtrapolator(final FieldTLE<T> tle,
  271.                                                    final AttitudeProvider attitudeProvider,
  272.                                                    final T mass,
  273.                                                    final T[] parameters) {
  274.         return selectExtrapolator(tle, attitudeProvider, mass,
  275.                 DataContext.getDefault().getFrames().getTEME(), parameters);
  276.     }

  278.     /** Selects the extrapolator to use with the selected TLE.
  279.      *
  280.      * @param tle the TLE to propagate.
  281.      * @param attitudeProvider provider for attitude computation
  282.      * @param mass spacecraft mass (kg)
  283.      * @param teme the TEME frame to use for propagation.
  284.      * @param parameters SGP4 and SDP4 model parameters
  285.      * @return the correct propagator.
  286.      * @param <T> elements type
  287.      */
  288.     public static <T extends CalculusFieldElement<T>> FieldTLEPropagator<T> selectExtrapolator(final FieldTLE<T> tle,
  289.                                                    final AttitudeProvider attitudeProvider,
  290.                                                    final T mass,
  291.                                                    final Frame teme,
  292.                                                    final T[] parameters) {

  294.         final T a1 = tle.getMeanMotion().multiply(60.0).reciprocal().multiply(TLEConstants.XKE).pow(TLEConstants.TWO_THIRD);
  295.         final T cosi0 = FastMath.cos(tle.getI());
  296.         final T temp1 = cosi0.multiply(cosi0.multiply(3.0)).subtract(1.0).multiply(1.5 * TLEConstants.CK2);
  297.         final T temp2 = tle.getE().multiply(tle.getE()).negate().add(1.0).pow(-1.5);
  298.         final T temp = temp1.multiply(temp2);
  299.         final T delta1 = temp.divide(a1.multiply(a1));
  300.         final T a0 = a1.multiply(delta1.multiply(delta1.multiply(
  301.                         delta1.multiply(134.0 / 81.0).add(1.0)).add(TLEConstants.ONE_THIRD)).negate().add(1.0));
  302.         final T delta0 = temp.divide(a0.multiply(a0));

  304.         // recover original mean motion :
  305.         final T xn0dp = tle.getMeanMotion().multiply(60.0).divide(delta0.add(1.0));

  307.         // Period >= 225 minutes is deep space
  308.         if (MathUtils.TWO_PI / (xn0dp.multiply(TLEConstants.MINUTES_PER_DAY).getReal()) >= (1.0 / 6.4)) {
  309.             return new FieldDeepSDP4<>(tle, attitudeProvider, mass, teme, parameters);
  310.         } else {
  311.             return new FieldSGP4<>(tle, attitudeProvider, mass, teme, parameters);
  312.         }
  313.     }

  315.     /** Get the Earth gravity coefficient used for TLE propagation.
  316.      * @return the Earth gravity coefficient.
  317.      */
  318.     public static double getMU() {
  319.         return TLEConstants.MU;
  320.     }

  322.     /** Get the extrapolated position and velocity from an initial TLE.
  323.      * @param date the final date
  324.      * @param parameters values of the model
  325.      * @return the final PVCoordinates
  326.      */
  327.     public FieldPVCoordinates<T> getPVCoordinates(final FieldAbsoluteDate<T> date, final T[] parameters) {

  329.         sxpPropagate(date.durationFrom(tle.getDate()).divide(60.0), parameters);

  331.         // Compute PV with previous calculated parameters
  332.         return computePVCoordinates();
  333.     }

  335.     /** Computation of the first commons parameters.
  336.      * @param parameters SGP4 and SDP4 model parameters
  337.      */
  338.     private void initializeCommons(final T[] parameters) {

  340.         final T zero = tle.getDate().getField().getZero();
  341.         final T bStar = parameters[0];
  342.         final T a1 = tle.getMeanMotion().multiply(60.0).reciprocal().multiply(TLEConstants.XKE).pow(TLEConstants.TWO_THIRD);
  343.         cosi0 = FastMath.cos(tle.getI());
  344.         theta2 = cosi0.multiply(cosi0);
  345.         final T x3thm1 = theta2.multiply(3.0).subtract(1.0);
  346.         e0sq = tle.getE().square();
  347.         beta02 = e0sq.negate().add(1.0);
  348.         beta0 = FastMath.sqrt(beta02);
  349.         final T tval = x3thm1.multiply(1.5 * TLEConstants.CK2).divide(beta0.multiply(beta02));
  350.         final T delta1 = tval.divide(a1.multiply(a1));
  351.         final T a0 = a1.multiply(delta1.multiply(
  352.                      delta1.multiply(134.0 / 81.0).add(1.0).multiply(delta1).add(TLEConstants.ONE_THIRD)).negate().add(1.0));
  353.         final T delta0 = tval.divide(a0.multiply(a0));

  355.         // recover original mean motion and semi-major axis :
  356.         xn0dp = tle.getMeanMotion().multiply(60.0).divide(delta0.add(1.0));
  357.         a0dp = a0.divide(delta0.negate().add(1.0));

  359.         // Values of s and qms2t :
  360.         s4 = zero.newInstance(TLEConstants.S);  // unmodified value for s
  361.         T q0ms24 = zero.newInstance(TLEConstants.QOMS2T); // unmodified value for q0ms2T

  363.         perige = a0dp.multiply(tle.getE().negate().add(1.0)).subtract(TLEConstants.NORMALIZED_EQUATORIAL_RADIUS).multiply(
  364.                                                                                                 TLEConstants.EARTH_RADIUS); // perige

  366.         //  For perigee below 156 km, the values of s and qoms2t are changed :
  367.         if (perige.getReal() < 156.0) {
  368.             if (perige.getReal() <= 98.0) {
  369.                 s4 = zero.newInstance(20.0);
  370.             } else {
  371.                 s4 = perige.subtract(78.0);
  372.             }
  373.             final T temp_val = s4.negate().add(120.0).multiply(TLEConstants.NORMALIZED_EQUATORIAL_RADIUS / TLEConstants.EARTH_RADIUS);
  374.             final T temp_val_squared = temp_val.multiply(temp_val);
  375.             q0ms24 = temp_val_squared.square();
  376.             s4 = s4.divide(TLEConstants.EARTH_RADIUS).add(TLEConstants.NORMALIZED_EQUATORIAL_RADIUS); // new value for q0ms2T and s
  377.         }

  379.         final T pinv = a0dp.multiply(beta02).reciprocal();
  380.         final T pinvsq = pinv.square();
  381.         tsi = a0dp.subtract(s4).reciprocal();
  382.         eta = a0dp.multiply(tle.getE()).multiply(tsi);
  383.         etasq = eta.square();
  384.         eeta = tle.getE().multiply(eta);

  386.         final T psisq = etasq.negate().add(1.0).abs(); // abs because pow 3.5 needs positive value
  387.         final T tsi_squared = tsi.multiply(tsi);
  388.         coef = q0ms24.multiply(tsi_squared.square());
  389.         coef1 = coef.divide(psisq.pow(3.5));

  391.         // C2 and C1 coefficients computation :
  392.         c2 = coef1.multiply(xn0dp).multiply(a0dp.multiply(
  393.                            etasq.multiply(1.5).add(eeta.multiply(etasq.add(4.0))).add(1.0)).add(
  394.                            tsi.divide(psisq).multiply(x3thm1).multiply(0.75 * TLEConstants.CK2).multiply(
  395.                            etasq.multiply(etasq.add(8.0)).multiply(3.0).add(8.0))));
  396.         c1 = bStar.multiply(c2);
  397.         sini0 = FastMath.sin(tle.getI());

  399.         final T x1mth2 = theta2.negate().add(1.0);

  401.         // C4 coefficient computation :
  402.         c4 = xn0dp.multiply(coef1).multiply(a0dp).multiply(2.0).multiply(beta02).multiply(
  403.                            eta.multiply(etasq.multiply(0.5).add(2.0)).add(tle.getE().multiply(etasq.multiply(2.0).add(0.5))).subtract(
  404.                            tsi.divide(a0dp.multiply(psisq)).multiply(2 * TLEConstants.CK2).multiply(
  405.                            x3thm1.multiply(-3).multiply(etasq.multiply(eeta.multiply(-0.5).add(1.5)).add(eeta.multiply(-2.0)).add(1.0)).add(
  406.                            x1mth2.multiply(0.75).multiply(etasq.multiply(2.0).subtract(eeta.multiply(etasq.add(1.0)))).multiply(FastMath.cos(tle.getPerigeeArgument().multiply(2.0)))))));

  408.         final T theta4 = theta2.multiply(theta2);
  409.         final T temp1  = pinvsq.multiply(xn0dp).multiply(3 * TLEConstants.CK2);
  410.         final T temp2  = temp1.multiply(pinvsq).multiply(TLEConstants.CK2);
  411.         final T temp3  = pinvsq.multiply(pinvsq).multiply(xn0dp).multiply(1.25 * TLEConstants.CK4);

  413.         // atmospheric and gravitation coefs :(Mdf and OMEGAdf)
  414.         xmdot = xn0dp.add(
  415.                 temp1.multiply(0.5).multiply(beta0).multiply(x3thm1)).add(
  416.                 temp2.multiply(0.0625).multiply(beta0).multiply(
  417.                 theta2.multiply(78.0).negate().add(13.0).add(theta4.multiply(137.0))));

  419.         final T x1m5th = theta2.multiply(5.0).negate().add(1.0);

  421.         omgdot = temp1.multiply(-0.5).multiply(x1m5th).add(
  422.                  temp2.multiply(0.0625).multiply(theta2.multiply(114.0).negate().add(
  423.                  theta4.multiply(395.0)).add(7.0))).add(
  424.                  temp3.multiply(theta2.multiply(36.0).negate().add(theta4.multiply(49.0)).add(3.0)));

  426.         final T xhdot1 = temp1.negate().multiply(cosi0);

  428.         xnodot = xhdot1.add(temp2.multiply(0.5).multiply(theta2.multiply(19.0).negate().add(4.0)).add(
  429.                  temp3.multiply(2.0).multiply(theta2.multiply(7.0).negate().add(3.0))).multiply(cosi0));
  430.         xnodcf = beta02.multiply(xhdot1).multiply(c1).multiply(3.5);
  431.         t2cof = c1.multiply(1.5);

  433.     }

  435.     /** Retrieves the position and velocity.
  436.      * @return the computed PVCoordinates.
  437.      */
  438.     private FieldPVCoordinates<T> computePVCoordinates() {

  440.         final T zero = tle.getDate().getField().getZero();
  441.         // Long period periodics
  442.         final T axn = e.multiply(FastMath.cos(omega));
  443.         T temp = a.multiply(e.multiply(e).negate().add(1.0)).reciprocal();
  444.         final T xlcof = sini0.multiply(0.125 * TLEConstants.A3OVK2).multiply(
  445.                              cosi0.multiply(5.0).add(3.0).divide(cosi0.add(1.0)));
  446.         final T aycof = sini0.multiply(0.25 * TLEConstants.A3OVK2);
  447.         final T xll   = temp.multiply(xlcof).multiply(axn);
  448.         final T aynl  = temp.multiply(aycof);
  449.         final T xlt   = xl.add(xll);
  450.         final T ayn   = e.multiply(FastMath.sin(omega)).add(aynl);
  451.         final T elsq  = axn.square().add(ayn.square());
  452.         final T capu  = MathUtils.normalizeAngle(xlt.subtract(xnode), zero.getPi());
  453.         T epw    = capu;
  454.         T ecosE  = zero;
  455.         T esinE  = zero;
  456.         T sinEPW = zero;
  457.         T cosEPW = zero;

  459.         // Dundee changes:  items dependent on cosio get recomputed:
  460.         final T cosi0Sq = cosi0.square();
  461.         final T x3thm1  = cosi0Sq.multiply(3.0).subtract(1.0);
  462.         final T x1mth2  = cosi0Sq.negate().add(1.0);
  463.         final T x7thm1  = cosi0Sq.multiply(7.0).subtract(1.0);

  465.         if (e.getReal() > (1 - 1e-6)) {
  466.             throw new OrekitException(OrekitMessages.TOO_LARGE_ECCENTRICITY_FOR_PROPAGATION_MODEL, e);
  467.         }

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

  473.             boolean doSecondOrderNewtonRaphson = true;

  475.             sinEPW = FastMath.sin( epw);
  476.             cosEPW = FastMath.cos( epw);
  477.             ecosE  = axn.multiply(cosEPW).add(ayn.multiply(sinEPW));
  478.             esinE  = axn.multiply(sinEPW).subtract(ayn.multiply(cosEPW));
  479.             final T f = capu.subtract(epw).add(esinE);
  480.             if (FastMath.abs(f.getReal()) < newtonRaphsonEpsilon) {
  481.                 break;
  482.             }
  483.             final T fdot = ecosE.negate().add(1.0);
  484.             T delta_epw = f.divide(fdot);
  485.             if (j == 0) {
  486.                 final T maxNewtonRaphson = e.abs().multiply(1.25);
  487.                 doSecondOrderNewtonRaphson = false;
  488.                 if (delta_epw.getReal() > maxNewtonRaphson.getReal()) {
  489.                     delta_epw = maxNewtonRaphson;
  490.                 } else if (delta_epw.getReal() < -maxNewtonRaphson.getReal()) {
  491.                     delta_epw = maxNewtonRaphson.negate();
  492.                 } else {
  493.                     doSecondOrderNewtonRaphson = true;
  494.                 }
  495.             }
  496.             if (doSecondOrderNewtonRaphson) {
  497.                 delta_epw = f.divide(fdot.add(esinE.multiply(0.5).multiply(delta_epw)));
  498.             }
  499.             epw = epw.add(delta_epw);
  500.         }

  502.         // Short period preliminary quantities
  503.         temp = elsq.negate().add(1.0);
  504.         final T pl = a.multiply(temp);
  505.         final T r  = a.multiply(ecosE.negate().add(1.0));
  506.         T temp2 = a.divide(r);
  507.         final T betal = FastMath.sqrt(temp);
  508.         temp = esinE.divide(betal.add(1.0));
  509.         final T cosu  = temp2.multiply(cosEPW.subtract(axn).add(ayn.multiply(temp)));
  510.         final T sinu  = temp2.multiply(sinEPW.subtract(ayn).subtract(axn.multiply(temp)));
  511.         final T u     = FastMath.atan2(sinu, cosu);
  512.         final T sin2u = sinu.multiply(cosu).multiply(2.0);
  513.         final T cos2u = cosu.multiply(cosu).multiply(2.0).subtract(1.0);
  514.         final T temp1 = pl.reciprocal().multiply(TLEConstants.CK2);
  515.         temp2         = temp1.divide(pl);

  517.         // Update for short periodics
  518.         final T rk = r.multiply(temp2.multiply(betal).multiply(x3thm1).multiply(-1.5).add(1.0)).add(
  519.                      temp1.multiply(x1mth2).multiply(cos2u).multiply(0.5));
  520.         final T uk = u.subtract(temp2.multiply(x7thm1).multiply(sin2u).multiply(0.25));
  521.         final T xnodek = xnode.add(temp2.multiply(cosi0).multiply(sin2u).multiply(1.5));
  522.         final T xinck = i.add(temp2.multiply(cosi0).multiply(sini0).multiply(cos2u).multiply(1.5));

  524.         // Orientation vectors
  525.         final T sinuk  = FastMath.sin(uk);
  526.         final T cosuk  = FastMath.cos(uk);
  527.         final T sinik  = FastMath.sin(xinck);
  528.         final T cosik  = FastMath.cos(xinck);
  529.         final T sinnok = FastMath.sin(xnodek);
  530.         final T cosnok = FastMath.cos(xnodek);
  531.         final T xmx    = sinnok.negate().multiply(cosik);
  532.         final T xmy    = cosnok.multiply(cosik);
  533.         final T ux     = xmx.multiply(sinuk).add(cosnok.multiply(cosuk));
  534.         final T uy     = xmy.multiply(sinuk).add(sinnok.multiply(cosuk));
  535.         final T uz     = sinik.multiply(sinuk);

  537.         // Position and velocity
  538.         final T cr = rk.multiply(1000 * TLEConstants.EARTH_RADIUS);
  539.         final FieldVector3D<T> pos = new FieldVector3D<>(cr.multiply(ux), cr.multiply(uy), cr.multiply(uz));

  541.         final T sqrtA  = FastMath.sqrt(a);
  542.         final T rdot   = sqrtA.multiply(esinE.divide(r)).multiply(TLEConstants.XKE);
  543.         final T rfdot  = FastMath.sqrt(pl).divide(r).multiply(TLEConstants.XKE);
  544.         final T xn     = a.multiply(sqrtA).reciprocal().multiply(TLEConstants.XKE);
  545.         final T rdotk  = rdot.subtract(xn.multiply(temp1).multiply(x1mth2).multiply(sin2u));
  546.         final T rfdotk = rfdot.add(xn.multiply(temp1).multiply(x1mth2.multiply(cos2u).add(x3thm1.multiply(1.5))));
  547.         final T vx     = xmx.multiply(cosuk).subtract(cosnok.multiply(sinuk));
  548.         final T vy     = xmy.multiply(cosuk).subtract(sinnok.multiply(sinuk));
  549.         final T vz     = sinik.multiply(cosuk);

  551.         final double cv = 1000.0 * TLEConstants.EARTH_RADIUS / 60.0;
  552.         final FieldVector3D<T> vel = new FieldVector3D<>(rdotk.multiply(ux).add(rfdotk.multiply(vx)).multiply(cv),
  553.                                                           rdotk.multiply(uy).add(rfdotk.multiply(vy)).multiply(cv),
  554.                                                           rdotk.multiply(uz).add(rfdotk.multiply(vz)).multiply(cv));
  555.         return new FieldPVCoordinates<>(pos, vel);

  557.     }

  559.     /** {@inheritDoc} */
  560.     @Override
  561.     public List<ParameterDriver> getParametersDrivers() {
  562.         return tle.getParametersDrivers();
  563.     }

  565.     /** Initialization proper to each propagator (SGP or SDP).
  566.      * @param parameters model parameters
  567.      */
  568.     protected abstract void sxpInitialize(T[] parameters);

  570.     /** Propagation proper to each propagator (SGP or SDP).
  571.      * @param t the offset from initial epoch (min)
  572.      * @param parameters model parameters
  573.      */
  574.     protected abstract void sxpPropagate(T t, T[] parameters);

  576.     /** {@inheritDoc}
  577.      * <p>
  578.      * For TLE propagator, calling this method is only recommended
  579.      * for covariance propagation when the new <code>state</code>
  580.      * differs from the previous one by only adding the additional
  581.      * state containing the derivatives.
  582.      * </p>
  583.      */
  584.     public void resetInitialState(final FieldSpacecraftState<T> state) {
  585.         super.resetInitialState(state);
  586.         resetTle(state);
  587.         tlesAndMasses = new TimeSpanMap<>(new Pair<>(tle, state.getMass()));
  588.     }

  590.     /** {@inheritDoc} */
  591.     protected void resetIntermediateState(final FieldSpacecraftState<T> state, final boolean forward) {
  592.         resetTle(state);
  593.         final Pair<FieldTLE<T>, T> tleAndMass = new Pair<>(tle, state.getMass());
  594.         if (forward) {
  595.             tlesAndMasses.addValidAfter(tleAndMass, state.getDate().toAbsoluteDate(), false);
  596.         } else {
  597.             tlesAndMasses.addValidBefore(tleAndMass, state.getDate().toAbsoluteDate(), false);
  598.         }
  599.         stateChanged(state);
  600.     }

  602.     /** Reset internal TLE from a SpacecraftState.
  603.      * @param state spacecraft state on which to base new TLE
  604.      */
  605.     private void resetTle(final FieldSpacecraftState<T> state) {
  606.         final TleGenerationAlgorithm algorithm = TLEPropagator.getDefaultTleGenerationAlgorithm(utc, teme);
  607.         final FieldTLE<T> newTle = algorithm.generate(state, tle);
  608.         initializeTle(newTle);
  609.     }

  611.     /** Initialize internal TLE.
  612.      * @param newTle tle to replace current one
  613.      */
  614.     private void initializeTle(final FieldTLE<T> newTle) {
  615.         tle = newTle;
  616.         final T[] parameters = tle.getParameters(tle.getDate().getField());
  617.         initializeCommons(parameters);
  618.         sxpInitialize(parameters);
  619.     }

  621.     /** {@inheritDoc} */
  622.     protected T getMass(final FieldAbsoluteDate<T> date) {
  623.         return tlesAndMasses.get(date.toAbsoluteDate()).getValue();
  624.     }

  626.     /** {@inheritDoc} */
  627.     public FieldOrbit<T> propagateOrbit(final FieldAbsoluteDate<T> date, final T[] parameters) {
  628.         final FieldTLE<T> closestTle = tlesAndMasses.get(date.toAbsoluteDate()).getKey();
  629.         if (!tle.equals(closestTle)) {
  630.             initializeTle(closestTle);
  631.         }
  632.         final T mu = date.getField().getZero().newInstance(TLEConstants.MU);
  633.         return new FieldCartesianOrbit<>(getPVCoordinates(date, parameters), teme, date, mu);
  634.     }

  636.     /** Get the underlying TLE.
  637.      * If there has been calls to #resetInitialState or #resetIntermediateState,
  638.      * it will not be the same as given to the constructor.
  639.      * @return underlying TLE
  640.      */
  641.     public FieldTLE<T> getTLE() {
  642.         return tle;
  643.     }

  645.     /** {@inheritDoc} */
  646.     public Frame getFrame() {
  647.         return teme;
  648.     }

  650. }
Created with JaCoCo 0.8.12.202403310830