GLONASSAnalyticalPropagator.java

  1. /* Copyright 2002-2020 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.gnss;

  19. import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
  20. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  21. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  22. import org.hipparchus.util.FastMath;
  23. import org.hipparchus.util.FieldSinCos;
  24. import org.hipparchus.util.MathArrays;
  25. import org.hipparchus.util.MathUtils;
  26. import org.hipparchus.util.Precision;
  27. import org.orekit.annotation.DefaultDataContext;
  28. import org.orekit.attitudes.AttitudeProvider;
  29. import org.orekit.data.DataContext;
  30. import org.orekit.errors.OrekitException;
  31. import org.orekit.errors.OrekitMessages;
  32. import org.orekit.frames.Frame;
  33. import org.orekit.frames.Frames;
  34. import org.orekit.orbits.CartesianOrbit;
  35. import org.orekit.orbits.Orbit;
  36. import org.orekit.propagation.Propagator;
  37. import org.orekit.propagation.SpacecraftState;
  38. import org.orekit.propagation.analytical.AbstractAnalyticalPropagator;
  39. import org.orekit.time.AbsoluteDate;
  40. import org.orekit.time.GLONASSDate;
  41. import org.orekit.time.TimeScale;
  42. import org.orekit.utils.IERSConventions;
  43. import org.orekit.utils.PVCoordinates;

  45. /**
  46.  * This class aims at propagating a GLONASS orbit from {@link GLONASSOrbitalElements}.
  47.  *
  48.  * @see <a href="http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD-GLONASS-CDMA-General.-Edition-1.0-2016.pdf">
  49.  *       GLONASS Interface Control Document</a>
  50.  *
  51.  * @author Bryan Cazabonne
  52.  * @since 10.0
  53.  *
  54.  */
  55. public class GLONASSAnalyticalPropagator extends AbstractAnalyticalPropagator {

  57.     // Constants
  58.     /** Constant 7.0 / 3.0. */
  59.     private static final double SEVEN_THIRD = 7.0 / 3.0;

  61.     /** Constant 7.0 / 6.0. */
  62.     private static final double SEVEN_SIXTH = 7.0 / 6.0;

  64.     /** Constant 7.0 / 24.0. */
  65.     private static final double SEVEN_24TH = 7.0 / 24.0;

  67.     /** Constant 49.0 / 72.0. */
  68.     private static final double FN_72TH = 49.0 / 72.0;

  70.     /** Value of the earth's rotation rate in rad/s. */
  71.     private static final double GLONASS_AV = 7.2921150e-5;

  73.     /** Mean value of inclination for Glonass orbit is equal to 63°. */
  74.     private static final double GLONASS_MEAN_INCLINATION = 64.8;

  76.     /** Mean value of Draconian period for Glonass orbit is equal to 40544s : 11 hours 15 minutes 44 seconds. */
  77.     private static final double GLONASS_MEAN_DRACONIAN_PERIOD = 40544;

  79.     /** Second degree zonal coefficient of normal potential. */
  80.     private static final double GLONASS_J20 = 1.08262575e-3;

  82.     /** Equatorial radius of Earth (m). */
  83.     private static final double GLONASS_EARTH_EQUATORIAL_RADIUS = 6378136;

  85.     // Data used to solve Kepler's equation
  86.     /** First coefficient to compute Kepler equation solver starter. */
  87.     private static final double A;

  89.     /** Second coefficient to compute Kepler equation solver starter. */
  90.     private static final double B;

  92.     static {
  93.         final double k1 = 3 * FastMath.PI + 2;
  94.         final double k2 = FastMath.PI - 1;
  95.         final double k3 = 6 * FastMath.PI - 1;
  96.         A  = 3 * k2 * k2 / k1;
  97.         B  = k3 * k3 / (6 * k1);
  98.     }

  100.     // Fields
  101.     /** The GLONASS orbital elements used. */
  102.     private final GLONASSOrbitalElements glonassOrbit;

  104.     /** The spacecraft mass (kg). */
  105.     private final double mass;

  107.     /** The ECI frame used for GLONASS propagation. */
  108.     private final Frame eci;

  110.     /** The ECEF frame used for GLONASS propagation. */
  111.     private final Frame ecef;

  113.     /** Data context for propagation. */
  114.     private final DataContext dataContext;

  116.     /**
  117.      * This nested class aims at building a GLONASSPropagator.
  118.      * <p>It implements the classical builder pattern.</p>
  119.      *
  120.      */
  121.     public static class Builder {

  123.         // Required parameter
  124.         /** The GLONASS orbital elements. */
  125.         private final GLONASSOrbitalElements orbit;

  127.         // Optional parameters
  128.         /** The attitude provider. */
  129.         private AttitudeProvider attitudeProvider;
  130.         /** The mass. */
  131.         private double mass = DEFAULT_MASS;
  132.         /** The ECI frame. */
  133.         private Frame eci  = null;
  134.         /** The ECEF frame. */
  135.         private Frame ecef = null;
  136.         /** Data context. */
  137.         private DataContext dataContext;

  139.         /** Initializes the builder.
  140.          * <p>The GLONASS orbital elements is the only requested parameter to build a GLONASSPropagator.</p>
  141.          * <p>The attitude provider is set by default to the
  142.          *  {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the
  143.          *  default data context.<br>
  144.          * The mass is set by default to the
  145.          *  {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.<br>
  146.          * The data context is by default to the
  147.          *  {@link DataContext#getDefault() default data context}.<br>
  148.          * The ECI frame is set by default to the
  149.          *  {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data
  150.          *  context.<br>
  151.          * The ECEF frame is set by default to the
  152.          *  {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP
  153.          *  CIO/2010-based ITRF simple EOP} in the default data context.
  154.          * </p>
  155.          *
  156.          * <p>This constructor uses the {@link DataContext#getDefault() default data context}.
  157.          * Another data context can be set using
  158.          * {@code Builder(final GLONASSOrbitalElements gpsOrbElt, final DataContext dataContext)}</p>
  159.          *
  160.          * @param glonassOrbElt the GLONASS orbital elements to be used by the GLONASS propagator.
  161.          * @see #attitudeProvider(AttitudeProvider provider)
  162.          * @see #mass(double mass)
  163.          * @see #eci(Frame inertial)
  164.          * @see #ecef(Frame bodyFixed)
  165.          */
  166.         @DefaultDataContext
  167.         public Builder(final GLONASSOrbitalElements glonassOrbElt) {
  168.             this(glonassOrbElt, DataContext.getDefault());
  169.         }

  171.         /** Initializes the builder.
  172.          * <p>The GLONASS orbital elements is the only requested parameter to build a GLONASSPropagator.</p>
  173.          * <p>The attitude provider is set by default to the
  174.          *  {@link org.orekit.propagation.Propagator#getDefaultLaw(Frames)}.<br>
  175.          * The mass is set by default to the
  176.          *  {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.<br>
  177.          * The ECI frame is set by default to the
  178.          *  {@link Frames#getEME2000() EME2000 frame}.<br>
  179.          * The ECEF frame is set by default to the
  180.          *  {@link Frames#getITRF(IERSConventions, boolean) CIO/2010-based ITRF simple
  181.          *  EOP}.
  182.          * </p>
  183.          *
  184.          * @param glonassOrbElt the GLONASS orbital elements to be used by the GLONASS propagator.
  185.          * @param dataContext the data context to use for frames and time scales.
  186.          * @see #attitudeProvider(AttitudeProvider provider)
  187.          * @see #mass(double mass)
  188.          * @see #eci(Frame inertial)
  189.          * @see #ecef(Frame bodyFixed)
  190.          * @since 10.1
  191.          */
  192.         public Builder(final GLONASSOrbitalElements glonassOrbElt,
  193.                        final DataContext dataContext) {
  194.             this.orbit = glonassOrbElt;
  195.             this.dataContext = dataContext;
  196.             final Frames frames = dataContext.getFrames();
  197.             this.eci   = frames.getEME2000();
  198.             this.ecef  = frames.getITRF(IERSConventions.IERS_2010, true);
  199.             attitudeProvider = Propagator.getDefaultLaw(frames);
  200.         }

  202.         /** Sets the attitude provider.
  203.          *
  204.          * @param userProvider the attitude provider
  205.          * @return the updated builder
  206.          */
  207.         public Builder attitudeProvider(final AttitudeProvider userProvider) {
  208.             this.attitudeProvider = userProvider;
  209.             return this;
  210.         }

  212.         /** Sets the mass.
  213.          *
  214.          * @param userMass the mass (in kg)
  215.          * @return the updated builder
  216.          */
  217.         public Builder mass(final double userMass) {
  218.             this.mass = userMass;
  219.             return this;
  220.         }

  222.         /** Sets the Earth Centered Inertial frame used for propagation.
  223.          *
  224.          * @param inertial the ECI frame
  225.          * @return the updated builder
  226.          */
  227.         public Builder eci(final Frame inertial) {
  228.             this.eci = inertial;
  229.             return this;
  230.         }

  232.         /** Sets the Earth Centered Earth Fixed frame assimilated to the WGS84 ECEF.
  233.          *
  234.          * @param bodyFixed the ECEF frame
  235.          * @return the updated builder
  236.          */
  237.         public Builder ecef(final Frame bodyFixed) {
  238.             this.ecef = bodyFixed;
  239.             return this;
  240.         }

  242.         /**
  243.          * Sets the data context used by the propagator. Does not update the ECI or ECEF
  244.          * frames which must be done separately using {@link #eci(Frame)} and {@link
  245.          * #ecef(Frame)}.
  246.          *
  247.          * @param context used for propagation.
  248.          * @return the updated builder.
  249.          */
  250.         public Builder dataContext(final DataContext context) {
  251.             this.dataContext = context;
  252.             return this;
  253.         }

  255.         /** Finalizes the build.
  256.          *
  257.          * @return the built GLONASSPropagator
  258.          */
  259.         public GLONASSAnalyticalPropagator build() {
  260.             return new GLONASSAnalyticalPropagator(this);
  261.         }

  263.     }

  265.     /**
  266.      * Private constructor.
  267.      * @param builder the builder
  268.      */
  269.     private GLONASSAnalyticalPropagator(final Builder builder) {
  270.         super(builder.attitudeProvider);
  271.         this.dataContext = builder.dataContext;
  272.         // Stores the GLONASS orbital elements
  273.         this.glonassOrbit = builder.orbit;
  274.         // Sets the start date as the date of the orbital elements
  275.         setStartDate(glonassOrbit.getDate());
  276.         // Sets the mass
  277.         this.mass = builder.mass;
  278.         // Sets the Earth Centered Inertial frame
  279.         this.eci  = builder.eci;
  280.         // Sets the Earth Centered Earth Fixed frame
  281.         this.ecef = builder.ecef;
  282.     }

  284.     /**
  285.      * Gets the PVCoordinates of the GLONASS SV in {@link #getECEF() ECEF frame}.
  286.      *
  287.      * <p>The algorithm is defined at Appendix M.1 from GLONASS Interface Control Document,
  288.      * with automatic differentiation added to compute velocity and
  289.      * acceleration.</p>
  290.      *
  291.      * @param date the computation date
  292.      * @return the GLONASS SV PVCoordinates in {@link #getECEF() ECEF frame}
  293.      */
  294.     public PVCoordinates propagateInEcef(final AbsoluteDate date) {

  296.         // Interval of prediction dTpr
  297.         final UnivariateDerivative2 dTpr = getdTpr(date);

  299.         // Zero
  300.         final UnivariateDerivative2 zero = dTpr.getField().getZero();

  302.         // The number of whole orbits "w" on a prediction interval
  303.         final UnivariateDerivative2 w = FastMath.floor(dTpr.divide(GLONASS_MEAN_DRACONIAN_PERIOD + glonassOrbit.getDeltaT()));

  305.         // Current inclination
  306.         final UnivariateDerivative2 i = zero.add(GLONASS_MEAN_INCLINATION / 180 * GLONASSOrbitalElements.GLONASS_PI + glonassOrbit.getDeltaI());

  308.         // Eccentricity
  309.         final UnivariateDerivative2 e = zero.add(glonassOrbit.getE());

  311.         // Mean draconique period in orbite w+1 and mean motion
  312.         final UnivariateDerivative2 tDR = w.multiply(2.0).add(1.0).multiply(glonassOrbit.getDeltaTDot()).
  313.                                           add(glonassOrbit.getDeltaT()).
  314.                                           add(GLONASS_MEAN_DRACONIAN_PERIOD);
  315.         final UnivariateDerivative2 n = tDR.divide(2.0 * GLONASSOrbitalElements.GLONASS_PI).reciprocal();

  317.         // Semi-major axis : computed by successive approximation
  318.         final UnivariateDerivative2 sma = computeSma(tDR, i, e);

  320.         // (ae / p)^2 term
  321.         final UnivariateDerivative2 p     = sma.multiply(e.multiply(e).negate().add(1.0));
  322.         final UnivariateDerivative2 aeop  = p.divide(GLONASS_EARTH_EQUATORIAL_RADIUS).reciprocal();
  323.         final UnivariateDerivative2 aeop2 = aeop.multiply(aeop);

  325.         // Current longitude of the ascending node
  326.         final UnivariateDerivative2 lambda = computeLambda(dTpr, n, aeop2, i);

  328.         // Current argument of perigee
  329.         final UnivariateDerivative2 pa = computePA(dTpr, n, aeop2, i);

  331.         // Mean longitude at the instant the spacecraft passes the current ascending node
  332.         final UnivariateDerivative2 tanPAo2 = FastMath.tan(pa.divide(2.0));
  333.         final UnivariateDerivative2 coef    = tanPAo2.multiply(FastMath.sqrt(e.negate().add(1.0).divide(e.add(1.0))));
  334.         final UnivariateDerivative2 e0      = FastMath.atan(coef).multiply(2.0).negate();
  335.         final UnivariateDerivative2 m1      = pa.add(e0).subtract(FastMath.sin(e0).multiply(e));

  337.         // Current mean longitude
  338.         final UnivariateDerivative2 correction = dTpr.
  339.                                                  subtract(w.multiply(GLONASS_MEAN_DRACONIAN_PERIOD + glonassOrbit.getDeltaT())).
  340.                                                  subtract(w.multiply(w).multiply(glonassOrbit.getDeltaTDot()));
  341.         final UnivariateDerivative2 m = m1.add(n.multiply(correction));

  343.         // Take into consideration the periodic perturbations
  344.         final FieldSinCos<UnivariateDerivative2> scPa = FastMath.sinCos(pa);
  345.         final UnivariateDerivative2 h = e.multiply(scPa.sin());
  346.         final UnivariateDerivative2 l = e.multiply(scPa.cos());
  347.         // δa1
  348.         final UnivariateDerivative2[] d1 = getParameterDifferentials(sma, i, h, l, m1);
  349.         // δa2
  350.         final UnivariateDerivative2[] d2 = getParameterDifferentials(sma, i, h, l, m);
  351.         // Apply corrections
  352.         final UnivariateDerivative2 smaCorr    = sma.add(d2[0]).subtract(d1[0]);
  353.         final UnivariateDerivative2 hCorr      = h.add(d2[1]).subtract(d1[1]);
  354.         final UnivariateDerivative2 lCorr      = l.add(d2[2]).subtract(d1[2]);
  355.         final UnivariateDerivative2 lambdaCorr = lambda.add(d2[3]).subtract(d1[3]);
  356.         final UnivariateDerivative2 iCorr      = i.add(d2[4]).subtract(d1[4]);
  357.         final UnivariateDerivative2 mCorr      = m.add(d2[5]).subtract(d1[5]);
  358.         final UnivariateDerivative2 eCorr      = FastMath.sqrt(hCorr.multiply(hCorr).add(lCorr.multiply(lCorr)));
  359.         final UnivariateDerivative2 paCorr;
  360.         if (eCorr.getValue() == 0.) {
  361.             paCorr = zero;
  362.         } else {
  363.             if (lCorr.getValue() == eCorr.getValue()) {
  364.                 paCorr = zero.add(0.5 * GLONASSOrbitalElements.GLONASS_PI);
  365.             } else if (lCorr.getValue() == -eCorr.getValue()) {
  366.                 paCorr = zero.add(-0.5 * GLONASSOrbitalElements.GLONASS_PI);
  367.             } else {
  368.                 paCorr = FastMath.atan2(hCorr, lCorr);
  369.             }
  370.         }

  372.         // Eccentric Anomaly
  373.         final UnivariateDerivative2 mk = mCorr.subtract(paCorr);
  374.         final UnivariateDerivative2 ek = getEccentricAnomaly(mk, eCorr);

  376.         // True Anomaly
  377.         final UnivariateDerivative2 vk =  getTrueAnomaly(ek, eCorr);

  379.         // Argument of Latitude
  380.         final UnivariateDerivative2 phik = vk.add(paCorr);

  382.         // Corrected Radius
  383.         final UnivariateDerivative2 pCorr = smaCorr.multiply(eCorr.multiply(eCorr).negate().add(1.0));
  384.         final UnivariateDerivative2 rk    = pCorr.divide(eCorr.multiply(FastMath.cos(vk)).add(1.0));

  386.         // Positions in orbital plane
  387.         final FieldSinCos<UnivariateDerivative2> scPhik = FastMath.sinCos(phik);
  388.         final UnivariateDerivative2 xk = scPhik.cos().multiply(rk);
  389.         final UnivariateDerivative2 yk = scPhik.sin().multiply(rk);

  391.         // Coordinates of position
  392.         final FieldSinCos<UnivariateDerivative2> scL = FastMath.sinCos(lambdaCorr);
  393.         final FieldSinCos<UnivariateDerivative2> scI = FastMath.sinCos(iCorr);
  394.         final FieldVector3D<UnivariateDerivative2> positionwithDerivatives =
  395.                         new FieldVector3D<>(xk.multiply(scL.cos()).subtract(yk.multiply(scL.sin()).multiply(scI.cos())),
  396.                                             xk.multiply(scL.sin()).add(yk.multiply(scL.cos()).multiply(scI.cos())),
  397.                                             yk.multiply(scI.sin()));

  399.         return new PVCoordinates(new Vector3D(positionwithDerivatives.getX().getValue(),
  400.                                               positionwithDerivatives.getY().getValue(),
  401.                                               positionwithDerivatives.getZ().getValue()),
  402.                                  new Vector3D(positionwithDerivatives.getX().getFirstDerivative(),
  403.                                               positionwithDerivatives.getY().getFirstDerivative(),
  404.                                               positionwithDerivatives.getZ().getFirstDerivative()),
  405.                                  new Vector3D(positionwithDerivatives.getX().getSecondDerivative(),
  406.                                               positionwithDerivatives.getY().getSecondDerivative(),
  407.                                               positionwithDerivatives.getZ().getSecondDerivative()));
  408.     }

  410.     /**
  411.      * Gets eccentric anomaly from mean anomaly.
  412.      * <p>The algorithm used to solve the Kepler equation has been published in:
  413.      * "Procedures for  solving Kepler's Equation", A. W. Odell and R. H. Gooding,
  414.      * Celestial Mechanics 38 (1986) 307-334</p>
  415.      * <p>It has been copied from the OREKIT library (KeplerianOrbit class).</p>
  416.      *
  417.      * @param mk the mean anomaly (rad)
  418.      * @param e the eccentricity
  419.      * @return the eccentric anomaly (rad)
  420.      */
  421.     private UnivariateDerivative2 getEccentricAnomaly(final UnivariateDerivative2 mk, final UnivariateDerivative2 e) {

  423.         // reduce M to [-PI PI] interval
  424.         final UnivariateDerivative2 reducedM = new UnivariateDerivative2(MathUtils.normalizeAngle(mk.getValue(), 0.0),
  425.                                                                          mk.getFirstDerivative(),
  426.                                                                          mk.getSecondDerivative());

  428.         // compute start value according to A. W. Odell and R. H. Gooding S12 starter
  429.         UnivariateDerivative2 ek;
  430.         if (FastMath.abs(reducedM.getValue()) < 1.0 / 6.0) {
  431.             if (FastMath.abs(reducedM.getValue()) < Precision.SAFE_MIN) {
  432.                 // this is an Orekit change to the S12 starter.
  433.                 // If reducedM is 0.0, the derivative of cbrt is infinite which induces NaN appearing later in
  434.                 // the computation. As in this case E and M are almost equal, we initialize ek with reducedM
  435.                 ek = reducedM;
  436.             } else {
  437.                 // this is the standard S12 starter
  438.                 ek = reducedM.add(reducedM.multiply(6).cbrt().subtract(reducedM).multiply(e));
  439.             }
  440.         } else {
  441.             if (reducedM.getValue() < 0) {
  442.                 final UnivariateDerivative2 w = reducedM.add(FastMath.PI);
  443.                 ek = reducedM.add(w.multiply(-A).divide(w.subtract(B)).subtract(FastMath.PI).subtract(reducedM).multiply(e));
  444.             } else {
  445.                 final UnivariateDerivative2 minusW = reducedM.subtract(FastMath.PI);
  446.                 ek = reducedM.add(minusW.multiply(A).divide(minusW.add(B)).add(FastMath.PI).subtract(reducedM).multiply(e));
  447.             }
  448.         }

  450.         final UnivariateDerivative2 e1 = e.negate().add(1.0);
  451.         final boolean noCancellationRisk = (e1.getValue() + ek.getValue() * ek.getValue() / 6) >= 0.1;

  453.         // perform two iterations, each consisting of one Halley step and one Newton-Raphson step
  454.         for (int j = 0; j < 2; ++j) {
  455.             final UnivariateDerivative2 f;
  456.             UnivariateDerivative2 fd;
  457.             final UnivariateDerivative2 fdd  = ek.sin().multiply(e);
  458.             final UnivariateDerivative2 fddd = ek.cos().multiply(e);
  459.             if (noCancellationRisk) {
  460.                 f  = ek.subtract(fdd).subtract(reducedM);
  461.                 fd = fddd.subtract(1).negate();
  462.             } else {
  463.                 f  = eMeSinE(ek, e).subtract(reducedM);
  464.                 final UnivariateDerivative2 s = ek.multiply(0.5).sin();
  465.                 fd = s.multiply(s).multiply(e.multiply(2.0)).add(e1);
  466.             }
  467.             final UnivariateDerivative2 dee = f.multiply(fd).divide(f.multiply(0.5).multiply(fdd).subtract(fd.multiply(fd)));

  469.             // update eccentric anomaly, using expressions that limit underflow problems
  470.             final UnivariateDerivative2 w = fd.add(dee.multiply(0.5).multiply(fdd.add(dee.multiply(fdd).divide(3))));
  471.             fd = fd.add(dee.multiply(fdd.add(dee.multiply(0.5).multiply(fdd))));
  472.             ek = ek.subtract(f.subtract(dee.multiply(fd.subtract(w))).divide(fd));
  473.         }

  475.         // expand the result back to original range
  476.         ek = ek.add(mk.getValue() - reducedM.getValue());

  478.         // Returns the eccentric anomaly
  479.         return ek;
  480.     }

  482.     /**
  483.      * Accurate computation of E - e sin(E).
  484.      *
  485.      * @param E eccentric anomaly
  486.      * @param ecc the eccentricity
  487.      * @return E - e sin(E)
  488.      */
  489.     private UnivariateDerivative2 eMeSinE(final UnivariateDerivative2 E, final UnivariateDerivative2 ecc) {
  490.         UnivariateDerivative2 x = E.sin().multiply(ecc.negate().add(1.0));
  491.         final UnivariateDerivative2 mE2 = E.negate().multiply(E);
  492.         UnivariateDerivative2 term = E;
  493.         UnivariateDerivative2 d    = E.getField().getZero();
  494.         // the inequality test below IS intentional and should NOT be replaced by a check with a small tolerance
  495.         for (UnivariateDerivative2 x0 = d.add(Double.NaN); !Double.valueOf(x.getValue()).equals(Double.valueOf(x0.getValue()));) {
  496.             d = d.add(2);
  497.             term = term.multiply(mE2.divide(d.multiply(d.add(1))));
  498.             x0 = x;
  499.             x = x.subtract(term);
  500.         }
  501.         return x;
  502.     }

  504.     /** Gets true anomaly from eccentric anomaly.
  505.     *
  506.     * @param ek the eccentric anomaly (rad)
  507.     * @param ecc the eccentricity
  508.     * @return the true anomaly (rad)
  509.     */
  510.     private UnivariateDerivative2 getTrueAnomaly(final UnivariateDerivative2 ek, final UnivariateDerivative2 ecc) {
  511.         final UnivariateDerivative2 svk = ek.sin().multiply(FastMath.sqrt( ecc.multiply(ecc).negate().add(1.0)));
  512.         final UnivariateDerivative2 cvk = ek.cos().subtract(ecc);
  513.         return svk.atan2(cvk);
  514.     }

  516.     /**
  517.      * Get the interval of prediction.
  518.      *
  519.      * @param date the considered date
  520.      * @return the duration from GLONASS orbit Reference epoch (s)
  521.      */
  522.     private UnivariateDerivative2 getdTpr(final AbsoluteDate date) {
  523.         final TimeScale glonass = dataContext.getTimeScales().getGLONASS();
  524.         final GLONASSDate tEnd = new GLONASSDate(date, glonass);
  525.         final GLONASSDate tSta = new GLONASSDate(glonassOrbit.getDate(), glonass);
  526.         final int n  = tEnd.getDayNumber();
  527.         final int na = tSta.getDayNumber();
  528.         final int deltaN;
  529.         if (na == 27) {
  530.             deltaN = n - na - FastMath.round((float) (n - na) / 1460) * 1460;
  531.         } else {
  532.             deltaN = n - na - FastMath.round((float) (n - na) / 1461) * 1461;
  533.         }
  534.         final UnivariateDerivative2 ti = new UnivariateDerivative2(tEnd.getSecInDay(), 1.0, 0.0);

  536.         return ti.subtract(glonassOrbit.getTime()).add(86400 * deltaN);
  537.     }

  539.     /**
  540.      * Computes the semi-major axis of orbit using technique of successive approximations.
  541.      * @param tDR mean draconique period (s)
  542.      * @param i current inclination (rad)
  543.      * @param e eccentricity
  544.      * @return the semi-major axis (m).
  545.      */
  546.     private UnivariateDerivative2 computeSma(final UnivariateDerivative2 tDR,
  547.                                              final UnivariateDerivative2 i,
  548.                                              final UnivariateDerivative2 e) {

  550.         // Zero
  551.         final UnivariateDerivative2 zero = tDR.getField().getZero();

  553.         // If one of the input parameter is equal to Double.NaN, an infinite loop can occur.
  554.         // In that case, we do not compute the value of the semi major axis.
  555.         // We decided to return a Double.NaN value instead.
  556.         if (Double.isNaN(tDR.getValue()) || Double.isNaN(i.getValue()) || Double.isNaN(e.getValue())) {
  557.             return zero.add(Double.NaN);
  558.         }

  560.         // Common parameters
  561.         final UnivariateDerivative2 sinI         = FastMath.sin(i);
  562.         final UnivariateDerivative2 sin2I        = sinI.multiply(sinI);
  563.         final UnivariateDerivative2 ome2         = e.multiply(e).negate().add(1.0);
  564.         final UnivariateDerivative2 ome2Pow3o2   = FastMath.sqrt(ome2).multiply(ome2);
  565.         final UnivariateDerivative2 pa           = zero.add(glonassOrbit.getPa());
  566.         final UnivariateDerivative2 cosPA        = FastMath.cos(pa);
  567.         final UnivariateDerivative2 opecosPA     = e.multiply(cosPA).add(1.0);
  568.         final UnivariateDerivative2 opecosPAPow2 = opecosPA.multiply(opecosPA);
  569.         final UnivariateDerivative2 opecosPAPow3 = opecosPAPow2.multiply(opecosPA);

  571.         // Initial approximation
  572.         UnivariateDerivative2 tOCK = tDR;

  574.         // Successive approximations
  575.         // The process of approximation ends when fulfilling the following condition: |a(n+1) - a(n)| < 1cm
  576.         UnivariateDerivative2 an   = zero;
  577.         UnivariateDerivative2 anp1 = zero;
  578.         boolean isLastStep = false;
  579.         while (!isLastStep) {

  581.             // a(n+1) computation
  582.             final UnivariateDerivative2 tOCKo2p     = tOCK.divide(2.0 * GLONASSOrbitalElements.GLONASS_PI);
  583.             final UnivariateDerivative2 tOCKo2pPow2 = tOCKo2p.multiply(tOCKo2p);
  584.             anp1 = FastMath.cbrt(tOCKo2pPow2.multiply(GLONASSOrbitalElements.GLONASS_MU));

  586.             // p(n+1) computation
  587.             final UnivariateDerivative2 p = anp1.multiply(ome2);

  589.             // Tock(n+1) computation
  590.             final UnivariateDerivative2 aeop  = p.divide(GLONASS_EARTH_EQUATORIAL_RADIUS).reciprocal();
  591.             final UnivariateDerivative2 aeop2 = aeop.multiply(aeop);
  592.             final UnivariateDerivative2 term1 = aeop2.multiply(GLONASS_J20).multiply(1.5);
  593.             final UnivariateDerivative2 term2 = sin2I.multiply(2.5).negate().add(2.0);
  594.             final UnivariateDerivative2 term3 = ome2Pow3o2.divide(opecosPAPow2);
  595.             final UnivariateDerivative2 term4 = opecosPAPow3.divide(ome2);
  596.             tOCK = tDR.divide(term1.multiply(term2.multiply(term3).add(term4)).negate().add(1.0));

  598.             // Check convergence
  599.             if (FastMath.abs(anp1.subtract(an).getReal()) <= 0.01) {
  600.                 isLastStep = true;
  601.             }

  603.             an = anp1;
  604.         }

  606.         return an;

  608.     }

  610.     /**
  611.      * Computes the current longitude of the ascending node.
  612.      * @param dTpr interval of prediction (s)
  613.      * @param n mean motion (rad/s)
  614.      * @param aeop2 square of the ratio between the radius of the ellipsoid and p, with p = sma * (1 - ecc²)
  615.      * @param i inclination (rad)
  616.      * @return the current longitude of the ascending node (rad)
  617.      */
  618.     private UnivariateDerivative2 computeLambda(final UnivariateDerivative2 dTpr,
  619.                                                 final UnivariateDerivative2 n,
  620.                                                 final UnivariateDerivative2 aeop2,
  621.                                                 final UnivariateDerivative2 i) {
  622.         final UnivariateDerivative2 cosI = FastMath.cos(i);
  623.         final UnivariateDerivative2 precession = aeop2.multiply(n).multiply(cosI).multiply(1.5 * GLONASS_J20);
  624.         return dTpr.multiply(precession.add(GLONASS_AV)).negate().add(glonassOrbit.getLambda());
  625.     }

  627.     /**
  628.      * Computes the current argument of perigee.
  629.      * @param dTpr interval of prediction (s)
  630.      * @param n mean motion (rad/s)
  631.      * @param aeop2 square of the ratio between the radius of the ellipsoid and p, with p = sma * (1 - ecc²)
  632.      * @param i inclination (rad)
  633.      * @return the current argument of perigee (rad)
  634.      */
  635.     private UnivariateDerivative2 computePA(final UnivariateDerivative2 dTpr,
  636.                                             final UnivariateDerivative2 n,
  637.                                             final UnivariateDerivative2 aeop2,
  638.                                             final UnivariateDerivative2 i) {
  639.         final UnivariateDerivative2 cosI  = FastMath.cos(i);
  640.         final UnivariateDerivative2 cos2I = cosI.multiply(cosI);
  641.         final UnivariateDerivative2 precession = aeop2.multiply(n).multiply(cos2I.multiply(5.0).negate().add(1.0)).multiply(0.75 * GLONASS_J20);
  642.         return dTpr.multiply(precession).negate().add(glonassOrbit.getPa());
  643.     }

  645.     /**
  646.      * Computes the differentials δa<sub>i</sub>.
  647.      * <p>
  648.      * The value of i depends of the type of longitude (i = 2 for the current mean longitude;
  649.      * i = 1 for the mean longitude at the instant the spacecraft passes the current ascending node)
  650.      * </p>
  651.      * @param a semi-major axis (m)
  652.      * @param i inclination (rad)
  653.      * @param h x component of the eccentricity (rad)
  654.      * @param l y component of the eccentricity (rad)
  655.      * @param m longitude (current or at the ascending node instant)
  656.      * @return the differentials of the orbital parameters
  657.      */
  658.     private UnivariateDerivative2[] getParameterDifferentials(final UnivariateDerivative2 a, final UnivariateDerivative2 i,
  659.                                                               final UnivariateDerivative2 h, final UnivariateDerivative2 l,
  660.                                                               final UnivariateDerivative2 m) {

  662.         // B constant
  663.         final UnivariateDerivative2 aeoa  = a.divide(GLONASS_EARTH_EQUATORIAL_RADIUS).reciprocal();
  664.         final UnivariateDerivative2 aeoa2 = aeoa.multiply(aeoa);
  665.         final UnivariateDerivative2 b     = aeoa2.multiply(1.5 * GLONASS_J20);

  667.         // Commons Parameters
  668.         final FieldSinCos<UnivariateDerivative2> scI   = FastMath.sinCos(i);
  669.         final FieldSinCos<UnivariateDerivative2> scLk  = FastMath.sinCos(m);
  670.         final FieldSinCos<UnivariateDerivative2> sc2Lk = FieldSinCos.sum(scLk, scLk);
  671.         final FieldSinCos<UnivariateDerivative2> sc3Lk = FieldSinCos.sum(scLk, sc2Lk);
  672.         final FieldSinCos<UnivariateDerivative2> sc4Lk = FieldSinCos.sum(sc2Lk, sc2Lk);
  673.         final UnivariateDerivative2 cosI   = scI.cos();
  674.         final UnivariateDerivative2 sinI   = scI.sin();
  675.         final UnivariateDerivative2 cosI2  = cosI.multiply(cosI);
  676.         final UnivariateDerivative2 sinI2  = sinI.multiply(sinI);
  677.         final UnivariateDerivative2 cosLk  = scLk.cos();
  678.         final UnivariateDerivative2 sinLk  = scLk.sin();
  679.         final UnivariateDerivative2 cos2Lk = sc2Lk.cos();
  680.         final UnivariateDerivative2 sin2Lk = sc2Lk.sin();
  681.         final UnivariateDerivative2 cos3Lk = sc3Lk.cos();
  682.         final UnivariateDerivative2 sin3Lk = sc3Lk.sin();
  683.         final UnivariateDerivative2 cos4Lk = sc4Lk.cos();
  684.         final UnivariateDerivative2 sin4Lk = sc4Lk.sin();

  686.         // h*cos(nLk), l*cos(nLk), h*sin(nLk) and l*sin(nLk)
  687.         // n = 1
  688.         final UnivariateDerivative2 hCosLk = h.multiply(cosLk);
  689.         final UnivariateDerivative2 hSinLk = h.multiply(sinLk);
  690.         final UnivariateDerivative2 lCosLk = l.multiply(cosLk);
  691.         final UnivariateDerivative2 lSinLk = l.multiply(sinLk);
  692.         // n = 2
  693.         final UnivariateDerivative2 hCos2Lk = h.multiply(cos2Lk);
  694.         final UnivariateDerivative2 hSin2Lk = h.multiply(sin2Lk);
  695.         final UnivariateDerivative2 lCos2Lk = l.multiply(cos2Lk);
  696.         final UnivariateDerivative2 lSin2Lk = l.multiply(sin2Lk);
  697.         // n = 3
  698.         final UnivariateDerivative2 hCos3Lk = h.multiply(cos3Lk);
  699.         final UnivariateDerivative2 hSin3Lk = h.multiply(sin3Lk);
  700.         final UnivariateDerivative2 lCos3Lk = l.multiply(cos3Lk);
  701.         final UnivariateDerivative2 lSin3Lk = l.multiply(sin3Lk);
  702.         // n = 4
  703.         final UnivariateDerivative2 hCos4Lk = h.multiply(cos4Lk);
  704.         final UnivariateDerivative2 hSin4Lk = h.multiply(sin4Lk);
  705.         final UnivariateDerivative2 lCos4Lk = l.multiply(cos4Lk);
  706.         final UnivariateDerivative2 lSin4Lk = l.multiply(sin4Lk);

  708.         // 1 - (3 / 2)*sin²i
  709.         final UnivariateDerivative2 om3o2xSinI2 = sinI2.multiply(1.5).negate().add(1.0);

  711.         // Compute Differentials
  712.         // δa
  713.         final UnivariateDerivative2 dakT1 = b.multiply(2.0).multiply(om3o2xSinI2).multiply(lCosLk.add(hSinLk));
  714.         final UnivariateDerivative2 dakT2 = b.multiply(sinI2).multiply(hSinLk.multiply(0.5).subtract(lCosLk.multiply(0.5)).
  715.                                                                      add(cos2Lk).add(lCos3Lk.multiply(3.5)).add(hSin3Lk.multiply(3.5)));
  716.         final UnivariateDerivative2 dak = dakT1.add(dakT2);

  718.         // δh
  719.         final UnivariateDerivative2 dhkT1 = b.multiply(om3o2xSinI2).multiply(sinLk.add(lSin2Lk.multiply(1.5)).subtract(hCos2Lk.multiply(1.5)));
  720.         final UnivariateDerivative2 dhkT2 = b.multiply(sinI2).multiply(0.25).multiply(sinLk.subtract(sin3Lk.multiply(SEVEN_THIRD)).add(lSin2Lk.multiply(5.0)).
  721.                                                                                     subtract(lSin4Lk.multiply(8.5)).add(hCos4Lk.multiply(8.5)).add(hCos2Lk));
  722.         final UnivariateDerivative2 dhkT3 = lSin2Lk.multiply(cosI2).multiply(b).multiply(0.5).negate();
  723.         final UnivariateDerivative2 dhk = dhkT1.subtract(dhkT2).add(dhkT3);

  725.         // δl
  726.         final UnivariateDerivative2 dlkT1 = b.multiply(om3o2xSinI2).multiply(cosLk.add(lCos2Lk.multiply(1.5)).add(hSin2Lk.multiply(1.5)));
  727.         final UnivariateDerivative2 dlkT2 = b.multiply(sinI2).multiply(0.25).multiply(cosLk.negate().subtract(cos3Lk.multiply(SEVEN_THIRD)).subtract(hSin2Lk.multiply(5.0)).
  728.                                                                                     subtract(lCos4Lk.multiply(8.5)).subtract(hSin4Lk.multiply(8.5)).add(lCos2Lk));
  729.         final UnivariateDerivative2 dlkT3 = hSin2Lk.multiply(cosI2).multiply(b).multiply(0.5);
  730.         final UnivariateDerivative2 dlk = dlkT1.subtract(dlkT2).add(dlkT3);

  732.         // δλ
  733.         final UnivariateDerivative2 dokT1 = b.negate().multiply(cosI);
  734.         final UnivariateDerivative2 dokT2 = lSinLk.multiply(3.5).subtract(hCosLk.multiply(2.5)).subtract(sin2Lk.multiply(0.5)).
  735.                                           subtract(lSin3Lk.multiply(SEVEN_SIXTH)).add(hCos3Lk.multiply(SEVEN_SIXTH));
  736.         final UnivariateDerivative2 dok = dokT1.multiply(dokT2);

  738.         // δi
  739.         final UnivariateDerivative2 dik = b.multiply(sinI).multiply(cosI).multiply(0.5).
  740.                         multiply(lCosLk.negate().add(hSinLk).add(cos2Lk).add(lCos3Lk.multiply(SEVEN_THIRD)).add(hSin3Lk.multiply(SEVEN_THIRD)));

  742.         // δL
  743.         final UnivariateDerivative2 dLkT1 = b.multiply(2.0).multiply(om3o2xSinI2).multiply(lSinLk.multiply(1.75).subtract(hCosLk.multiply(1.75)));
  744.         final UnivariateDerivative2 dLkT2 = b.multiply(sinI2).multiply(3.0).multiply(hCosLk.multiply(SEVEN_24TH).negate().subtract(lSinLk.multiply(SEVEN_24TH)).
  745.                                                                                    subtract(hCos3Lk.multiply(FN_72TH)).add(lSin3Lk.multiply(FN_72TH)).add(sin2Lk.multiply(0.25)));
  746.         final UnivariateDerivative2 dLkT3 = b.multiply(cosI2).multiply(lSinLk.multiply(3.5).subtract(hCosLk.multiply(2.5)).subtract(sin2Lk.multiply(0.5)).
  747.                                                                      subtract(lSin3Lk.multiply(SEVEN_SIXTH)).add(hCos3Lk.multiply(SEVEN_SIXTH)));
  748.         final UnivariateDerivative2 dLk = dLkT1.add(dLkT2).add(dLkT3);

  750.         // Final array
  751.         final UnivariateDerivative2[] differentials = MathArrays.buildArray(a.getField(), 6);
  752.         differentials[0] = dak.multiply(a);
  753.         differentials[1] = dhk;
  754.         differentials[2] = dlk;
  755.         differentials[3] = dok;
  756.         differentials[4] = dik;
  757.         differentials[5] = dLk;

  759.         return differentials;
  760.     }

  762.     /** {@inheritDoc} */
  763.     protected double getMass(final AbsoluteDate date) {
  764.         return mass;
  765.     }

  767.     /**
  768.      * Get the Earth gravity coefficient used for GLONASS propagation.
  769.      * @return the Earth gravity coefficient.
  770.      */
  771.     public static double getMU() {
  772.         return GLONASSOrbitalElements.GLONASS_MU;
  773.     }

  775.     /**
  776.      * Gets the underlying GLONASS orbital elements.
  777.      *
  778.      * @return the underlying GLONASS orbital elements
  779.      */
  780.     public GLONASSOrbitalElements getGLONASSOrbitalElements() {
  781.         return glonassOrbit;
  782.     }

  784.     /**
  785.      * Gets the Earth Centered Inertial frame used to propagate the orbit.
  786.      * @return the ECI frame
  787.      */
  788.     public Frame getECI() {
  789.         return eci;
  790.     }

  792.     /**
  793.      * Gets the Earth Centered Earth Fixed frame used to propagate GLONASS orbits.
  794.      * @return the ECEF frame
  795.      */
  796.     public Frame getECEF() {
  797.         return ecef;
  798.     }

  800.     /** {@inheritDoc} */
  801.     public Frame getFrame() {
  802.         return eci;
  803.     }

  805.     /** {@inheritDoc} */
  806.     public void resetInitialState(final SpacecraftState state) {
  807.         throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE);
  808.     }

  810.     /** {@inheritDoc} */
  811.     protected void resetIntermediateState(final SpacecraftState state, final boolean forward) {
  812.         throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE);
  813.     }

  815.     /** {@inheritDoc} */
  816.     protected Orbit propagateOrbit(final AbsoluteDate date) {
  817.         // Gets the PVCoordinates in ECEF frame
  818.         final PVCoordinates pvaInECEF = propagateInEcef(date);
  819.         // Transforms the PVCoordinates to ECI frame
  820.         final PVCoordinates pvaInECI = ecef.getTransformTo(eci, date).transformPVCoordinates(pvaInECEF);
  821.         // Returns the Cartesian orbit
  822.         return new CartesianOrbit(pvaInECI, eci, date, GLONASSOrbitalElements.GLONASS_MU);
  823.     }

  825. }
Created with JaCoCo 0.8.6.202009150832