GLONASSNumericalPropagator.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.numerical;

  18. import java.util.Arrays;

  19. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  20. import org.hipparchus.ode.ODEIntegrator;
  21. import org.hipparchus.ode.nonstiff.ClassicalRungeKuttaIntegrator;
  22. import org.hipparchus.util.FastMath;
  23. import org.hipparchus.util.MathUtils;
  24. import org.orekit.annotation.DefaultDataContext;
  25. import org.orekit.attitudes.Attitude;
  26. import org.orekit.attitudes.AttitudeProvider;
  27. import org.orekit.data.DataContext;
  28. import org.orekit.errors.OrekitException;
  29. import org.orekit.errors.OrekitMessages;
  30. import org.orekit.frames.Frame;
  31. import org.orekit.gnss.GLONASSEphemeris;
  32. import org.orekit.orbits.CartesianOrbit;
  33. import org.orekit.orbits.Orbit;
  34. import org.orekit.orbits.OrbitType;
  35. import org.orekit.orbits.PositionAngle;
  36. import org.orekit.propagation.PropagationType;
  37. import org.orekit.propagation.Propagator;
  38. import org.orekit.propagation.SpacecraftState;
  39. import org.orekit.propagation.analytical.gnss.GLONASSOrbitalElements;
  40. import org.orekit.propagation.integration.AbstractIntegratedPropagator;
  41. import org.orekit.propagation.integration.StateMapper;
  42. import org.orekit.time.AbsoluteDate;
  43. import org.orekit.time.GLONASSDate;
  44. import org.orekit.utils.AbsolutePVCoordinates;
  45. import org.orekit.utils.Constants;
  46. import org.orekit.utils.IERSConventions;
  47. import org.orekit.utils.PVCoordinates;
  48. import org.orekit.utils.TimeStampedPVCoordinates;

  49. /**
  50.  * This class propagates GLONASS orbits using numerical integration.
  51.  * <p>
  52.  * As recommended by the GLONASS Interface Control Document (ICD),
  53.  * a {@link ClassicalRungeKuttaIntegrator  4th order Runge-Kutta technique}
  54.  * shall be used to integrate the equations.
  55.  * </p>
  56.  * <p>
  57.  * Classical used of this orbit propagator is to compute GLONASS satellite
  58.  * coordinates from the navigation message.
  59.  * </p>
  60.  * <p>
  61.  * If the projections of luni-solar accelerations to axes of
  62.  * Greenwich geocentric coordinates {@link GLONASSOrbitalElements#getXDotDot() X''(tb)},
  63.  * {@link GLONASSOrbitalElements#getYDotDot() Y''(tb)} and {@link GLONASSOrbitalElements#getZDotDot() Z''(tb)}
  64.  * are available in the navigation message; a transformation is performed to convert these
  65.  * accelerations into the correct coordinate system. In the case where they are not
  66.  * available into the navigation message, these accelerations are computed.
  67.  * </p>
  68.  *
  69.  * @see <a href="http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD-GLONASS-CDMA-General.-Edition-1.0-2016.pdf">
  70.  *       GLONASS Interface Control Document</a>
  71.  *
  72.  * @author Bryan Cazabonne
  73.  *
  74.  */
  75. public class GLONASSNumericalPropagator extends AbstractIntegratedPropagator {

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

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

  80.     /** Value of the Earth's rotation rate in rad/s (See Ref). */
  81.     private static final double GLONASS_AV = 7.2921151467e-5;

  82.     // Data used to solve Kepler's equation
  83.     /** First coefficient to compute Kepler equation solver starter. */
  84.     private static final double A;

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

  87.     static {
  88.         final double k1 = 3 * FastMath.PI + 2;
  89.         final double k2 = FastMath.PI - 1;
  90.         final double k3 = 6 * FastMath.PI - 1;
  91.         A  = 3 * k2 * k2 / k1;
  92.         B  = k3 * k3 / (6 * k1);
  93.     }

  94.     /** The GLONASS orbital elements used. */
  95.     private final GLONASSOrbitalElements glonassOrbit;

  96.     /** Initial date in GLONASS form. */
  97.     private final GLONASSDate initDate;

  98.     /** The spacecraft mass (kg). */
  99.     private final double mass;

  100.     /** The ECI frame used for GLONASS propagation. */
  101.     private final Frame eci;

  102.     /** Direction cosines and distance of perturbing body: Moon.
  103.      * <p>
  104.      * <ul>
  105.      * <li>double[0] = ξ<sub>m</sub></li>
  106.      * <li>double[1] = η<sub>m</sub></li>
  107.      * <li>double[2] = ψ<sub>m</sub></li>
  108.      * <li>double[3] = r<sub>m</sub></li>
  109.      * </ul>
  110.      * </p>
  111.      */
  112.     private double[] moonElements;

  113.     /** Direction cosines and distance of perturbing body: Sun.
  114.      * <p>
  115.      * <ul>
  116.      * <li>double[0] = ξ<sub>s</sub></li>
  117.      * <li>double[1] = η<sub>s</sub></li>
  118.      * <li>double[2] = ψ<sub>s</sub></li>
  119.      * <li>double[3] = r<sub>s</sub></li>
  120.      * </ul>
  121.      * </p>
  122.      */
  123.     private double[] sunElements;

  124.     /** Flag for availability of projections of acceleration transmitted within the navigation message. */
  125.     private final boolean isAccAvailable;

  126.     /** Data context used for propagation. */
  127.     private final DataContext dataContext;

  128.     /**
  129.      * This nested class aims at building a GLONASSNumericalPropagator.
  130.      * <p>It implements the classical builder pattern.</p>
  131.      *
  132.      */
  133.     public static class Builder {

  134.         // Required parameter
  135.         /** The GLONASS orbital elements. */
  136.         private final GLONASSEphemeris orbit;

  137.         /** The 4th order Runge-Kutta integrator. */
  138.         private final ClassicalRungeKuttaIntegrator integrator;

  139.         /** Flag for availability of projections of acceleration transmitted within the navigation message. */
  140.         private final boolean isAccAvailable;

  141.         // Optional parameters
  142.         /** The attitude provider. */
  143.         private AttitudeProvider attitudeProvider;

  144.         /** The mass. */
  145.         private double mass = DEFAULT_MASS;

  146.         /** The ECI frame. */
  147.         private Frame eci  = null;

  148.         /** Data context for the propagator. */
  149.         private DataContext dataContext;

  150.         /**
  151.          * Initializes the builder.
  152.          * <p>The attitude provider is set by default to the
  153.          *  {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW} in the
  154.          *  default data context.<br>
  155.          * The mass is set by default to the
  156.          *  {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.<br>
  157.          * The data context is by default to the
  158.          *  {@link DataContext#getDefault() default data context}.<br>
  159.          * The ECI frame is set by default to the
  160.          *  {@link org.orekit.frames.Predefined#EME2000 EME2000 frame} in the default data
  161.          *  context.<br>
  162.          * </p>
  163.          *
  164.          * @param integrator 4th order Runge-Kutta as recommended by GLONASS ICD
  165.          * @param glonassOrbElt the GLONASS orbital elements to be used by the GLONASSNumericalPropagator.
  166.          * @param isAccAvailable flag for availability of the projections of accelerations transmitted within
  167.          *        the navigation message
  168.          * @see #attitudeProvider(AttitudeProvider provider)
  169.          * @see #mass(double mass)
  170.          * @see #eci(Frame inertial)
  171.          */
  172.         @DefaultDataContext
  173.         public Builder(final ClassicalRungeKuttaIntegrator integrator,
  174.                        final GLONASSEphemeris glonassOrbElt,
  175.                        final boolean isAccAvailable) {
  176.             this.isAccAvailable = isAccAvailable;
  177.             this.integrator     = integrator;
  178.             this.orbit          = glonassOrbElt;
  179.             this.dataContext    = DataContext.getDefault();
  180.             this.eci            = dataContext.getFrames().getEME2000();
  181.             attitudeProvider = Propagator.getDefaultLaw(dataContext.getFrames());
  182.         }

  183.         /**
  184.          * Sets the attitude provider.
  185.          *
  186.          * @param userProvider the attitude provider
  187.          * @return the updated builder
  188.          */
  189.         public Builder attitudeProvider(final AttitudeProvider userProvider) {
  190.             this.attitudeProvider = userProvider;
  191.             return this;
  192.         }

  193.         /**
  194.          * Sets the mass.
  195.          *
  196.          * @param userMass the mass (in kg)
  197.          * @return the updated builder
  198.          */
  199.         public Builder mass(final double userMass) {
  200.             this.mass = userMass;
  201.             return this;
  202.         }

  203.         /**
  204.          * Sets the Earth Centered Inertial frame used for propagation.
  205.          *
  206.          * @param inertial the ECI frame
  207.          * @return the updated builder
  208.          */
  209.         public Builder eci(final Frame inertial) {
  210.             this.eci = inertial;
  211.             return this;
  212.         }

  213.         /**
  214.          * Sets the data context used by the propagator. Does not update the ECI frame
  215.          * which must be done separately using {@link #eci(Frame)}.
  216.          *
  217.          * @param context used for propagation.
  218.          * @return the updated builder.
  219.          */
  220.         public Builder dataContext(final DataContext context) {
  221.             this.dataContext = context;
  222.             return this;
  223.         }

  224.         /**
  225.          * Finalizes the build.
  226.          *
  227.          * @return the built GPSPropagator
  228.          */
  229.         public GLONASSNumericalPropagator build() {
  230.             return new GLONASSNumericalPropagator(this);
  231.         }
  232.     }

  233.     /**
  234.      * Private constructor.
  235.      *
  236.      * @param builder the builder
  237.      */
  238.     public GLONASSNumericalPropagator(final Builder builder) {
  239.         super(builder.integrator, PropagationType.MEAN);
  240.         this.dataContext = builder.dataContext;
  241.         this.isAccAvailable = builder.isAccAvailable;
  242.         // Stores the GLONASS orbital elements
  243.         this.glonassOrbit = builder.orbit;
  244.         // Sets the Earth Centered Inertial frame
  245.         this.eci = builder.eci;
  246.         // Sets the mass
  247.         this.mass = builder.mass;
  248.         this.initDate = new GLONASSDate(
  249.                 glonassOrbit.getDate(),
  250.                 dataContext.getTimeScales().getGLONASS());

  251.         // Initialize state mapper
  252.         initMapper();
  253.         setInitialState();
  254.         setAttitudeProvider(builder.attitudeProvider);
  255.         setOrbitType(OrbitType.CARTESIAN);
  256.         // It is not meaningful for propagation in Cartesian parameters
  257.         setPositionAngleType(PositionAngle.TRUE);
  258.         setMu(GLONASSOrbitalElements.GLONASS_MU);

  259.         // As recommended by GLONASS ICD (2016), the direction cosines and distance
  260.         // of perturbing body are calculated one time (at tb).
  261.         if (!isAccAvailable) {
  262.             computeMoonElements(initDate);
  263.             computeSunElements(initDate);
  264.         }
  265.     }

  266.     /**
  267.      * Gets the underlying GLONASS orbital elements.
  268.      *
  269.      * @return the underlying GLONASS orbital elements
  270.      */
  271.     public GLONASSOrbitalElements getGLONASSOrbitalElements() {
  272.         return glonassOrbit;
  273.     }

  274.     /** {@inheritDoc} */
  275.     @Override
  276.     public SpacecraftState propagate(final AbsoluteDate date) {
  277.         // Spacecraft state in inertial frame
  278.         final SpacecraftState stateInInertial = super.propagate(date);

  279.         // Build the spacecraft state in inertial frame
  280.         final PVCoordinates pvInPZ90 = getPVInPZ90(stateInInertial);
  281.         final AbsolutePVCoordinates absPV = new AbsolutePVCoordinates(
  282.                 dataContext.getFrames().getPZ9011(IERSConventions.IERS_2010, true),
  283.                 stateInInertial.getDate(), pvInPZ90);
  284.         final TimeStampedPVCoordinates pvInInertial = absPV.getPVCoordinates(eci);
  285.         final SpacecraftState transformedState = new SpacecraftState(new CartesianOrbit(pvInInertial, eci, pvInInertial.getDate(), GLONASSOrbitalElements.GLONASS_MU),
  286.                                                                 stateInInertial.getAttitude(),
  287.                                                                 stateInInertial.getMass(), stateInInertial.getAdditionalStates());

  288.         return transformedState;
  289.     }

  290.     /**
  291.      * Set the initial state.
  292.      * <p>
  293.      * The initial conditions on position and velocity are in the ECEF coordinate system PZ-90.
  294.      * Previous to orbit integration, they must be transformed to an absolute inertial coordinate system.
  295.      * </p>
  296.      */
  297.     private void setInitialState() {

  298.         // Transform initial PV coordinates to an absolute inertial coordinate system.
  299.         final PVCoordinates pvInInertial = getPVInInertial(initDate);

  300.         // Create a new orbit
  301.         final Orbit orbit = new CartesianOrbit(pvInInertial,
  302.                                                eci, initDate.getDate(),
  303.                                                GLONASSOrbitalElements.GLONASS_MU);

  304.         // Reset the initial state to apply the transformation
  305.         resetInitialState(new SpacecraftState(orbit, mass));
  306.     }

  307.     /**
  308.      * This method computes the direction cosines and the distance used to
  309.      * compute the gravitational perturbations of the Moon.
  310.      *
  311.      * @param date the computation date in GLONASS scale
  312.      */
  313.     private void computeMoonElements(final GLONASSDate date) {

  314.         moonElements = new double[4];

  315.         // Constants
  316.         // Semi-major axis of the Moon's orbit (m)
  317.         final double am = 3.84385243e8;
  318.         // The Moon's orbit eccentricity
  319.         final double em = 0.054900489;
  320.         // Mean inclination of the Moon's orbit to the ecliptic (rad)
  321.         final double im = 0.0898041080;

  322.         // Computed parameters
  323.         // Time from epoch 2000 to the instant tb of GLONASS ephemeris broadcast
  324.         final double dtoJD = (glonassOrbit.getTime() - 10800.) / Constants.JULIAN_DAY;
  325.         final double t  = (date.getJD0() + dtoJD - 2451545.0) / 36525;
  326.         final double t2 = t * t;
  327.         // Mean inclination of Earth equator to ecliptic (rad)
  328.         final double eps = 0.4090926006 - 0.0002270711 * t;
  329.         // Mean longitude of the Moon's orbit perigee (rad)
  330.         final double gammaM = 1.4547885346 + 71.0176852437 * t - 0.0001801481 * t2;
  331.         // Mean longitude of the ascending node of the Moon (rad)
  332.         final double omegaM = 2.1824391966 - 33.7570459536 * t + 0.0000362262 * t2;
  333.         // Mean anomaly of the Moon (rad)
  334.         final double qm = 2.3555557435 + 8328.6914257190 * t + 0.0001545547 * t2;

  335.         // Commons parameters
  336.         final double cosOm = FastMath.cos(omegaM);
  337.         final double sinOm = FastMath.sin(omegaM);
  338.         final double cosIm = FastMath.cos(im);
  339.         final double sinIm = FastMath.sin(im);
  340.         final double cosEs = FastMath.cos(eps);
  341.         final double sinEs = FastMath.sin(eps);
  342.         final double cosGm = FastMath.cos(gammaM);
  343.         final double sinGm = FastMath.sin(gammaM);

  344.         // Intermediate parameters
  345.         final double psiStar = cosOm * sinIm;
  346.         final double etaStar = sinOm * sinIm;
  347.         final double epsStar = 1. - cosOm * cosOm * (1. - cosIm);
  348.         final double eps11 = sinOm * cosOm * (1. - cosIm);
  349.         final double eps12 = 1. - sinOm * sinOm * (1. - cosIm);
  350.         final double eta11 = epsStar * cosEs - psiStar * sinEs;
  351.         final double eta12 = eps11 * cosEs + etaStar * sinEs;
  352.         final double psi11 = epsStar * sinEs + psiStar * cosEs;
  353.         final double psi12 = eps11 * sinEs - etaStar * cosEs;

  354.         // Eccentric Anomaly
  355.         final double ek = getEccentricAnomaly(qm, em);

  356.         // True Anomaly
  357.         final double vk =  getTrueAnomaly(ek, em);
  358.         final double sinVk = FastMath.sin(vk);
  359.         final double cosVk = FastMath.cos(vk);

  360.         // Direction cosine
  361.         final double epsM = eps11 * (sinVk * cosGm + cosVk * sinGm) + eps12 * (cosVk * cosGm - sinVk * sinGm);
  362.         final double etaM = eta11 * (sinVk * cosGm + cosVk * sinGm) + eta12 * (cosVk * cosGm - sinVk * sinGm);
  363.         final double psiM = psi11 * (sinVk * cosGm + cosVk * sinGm) + psi12 * (cosVk * cosGm - sinVk * sinGm);

  364.         // Distance
  365.         final double rm = am * (1. - em * FastMath.cos(ek));

  366.         moonElements[0] = epsM;
  367.         moonElements[1] = etaM;
  368.         moonElements[2] = psiM;
  369.         moonElements[3] = rm;

  370.     }

  371.     /**
  372.      * This method computes the direction cosines and the distance used to
  373.      * compute the gravitational perturbations of the Sun.
  374.      *
  375.      * @param date the computation date in GLONASS scale
  376.      */
  377.     private void computeSunElements(final GLONASSDate date) {

  378.         sunElements = new double[4];

  379.         // Constants
  380.         //  Major semi-axis of the Earth’s orbit around the Sun (m)
  381.         final double as = 1.49598e11;
  382.         // The eccentricity of the Earth’s orbit around the Sun
  383.         final double es = 0.016719;

  384.         // Computed parameters
  385.         // Time from epoch 2000 to the instant tb of GLONASS ephemeris broadcast
  386.         final double dtoJD = (glonassOrbit.getTime() - 10800.) / Constants.JULIAN_DAY;
  387.         final double t  = (date.getJD0() + dtoJD - 2451545.0) / 36525;
  388.         final double t2 = t * t;
  389.         // Mean inclination of Earth equator to ecliptic (rad)
  390.         final double eps = 0.4090926006 - 0.0002270711 * t;
  391.         // Mean tropic longitude of the Sun orbit perigee (rad)
  392.         final double ws = -7.6281824375 + 0.0300101976 * t + 0.0000079741 * t2;
  393.         // Mean anomaly of the Sun (rad)
  394.         final double qs = 6.2400601269 + 628.3019551714 * t - 0.0000026820 * t2;

  395.         // Eccentric Anomaly
  396.         final double ek = getEccentricAnomaly(qs, es);

  397.         // True Anomaly
  398.         final double vk =  getTrueAnomaly(ek, es);
  399.         final double sinVk = FastMath.sin(vk);
  400.         final double cosVk = FastMath.cos(vk);

  401.         // Commons parameters
  402.         final double cosWs = FastMath.cos(ws);
  403.         final double sinWs = FastMath.sin(ws);
  404.         final double cosEs = FastMath.cos(eps);
  405.         final double sinEs = FastMath.sin(eps);

  406.         // Direction cosine
  407.         final double epsS = cosVk * cosWs - sinVk * sinWs;
  408.         final double etaS = cosEs * (sinVk * cosWs + cosVk * sinWs);
  409.         final double psiS = sinEs * (sinVk * cosWs + cosVk * sinWs);

  410.         // Distance
  411.         final double rs = as * (1. - es * FastMath.cos(ek));

  412.         sunElements[0] = epsS;
  413.         sunElements[1] = etaS;
  414.         sunElements[2] = psiS;
  415.         sunElements[3] = rs;

  416.     }

  417.     /**
  418.      * Computes the elliptic eccentric anomaly from the mean anomaly.
  419.      * <p>
  420.      * The algorithm used here for solving Kepler equation has been published
  421.      * in: "Procedures for  solving Kepler's Equation", A. W. Odell and
  422.      * R. H. Gooding, Celestial Mechanics 38 (1986) 307-334
  423.      * </p>
  424.      * <p>It has been copied from the OREKIT library (KeplerianOrbit class).</p>
  425.      *
  426.      * @param M mean anomaly (rad)
  427.      * @param e eccentricity
  428.      * @return E the eccentric anomaly
  429.      */
  430.     private double getEccentricAnomaly(final double M, final double e) {

  431.         // reduce M to [-PI PI) interval
  432.         final double reducedM = MathUtils.normalizeAngle(M, 0.0);

  433.         // compute start value according to A. W. Odell and R. H. Gooding S12 starter
  434.         double E;
  435.         if (FastMath.abs(reducedM) < 1.0 / 6.0) {
  436.             E = reducedM + e * (FastMath.cbrt(6 * reducedM) - reducedM);
  437.         } else {
  438.             if (reducedM < 0) {
  439.                 final double w = FastMath.PI + reducedM;
  440.                 E = reducedM + e * (A * w / (B - w) - FastMath.PI - reducedM);
  441.             } else {
  442.                 final double w = FastMath.PI - reducedM;
  443.                 E = reducedM + e * (FastMath.PI - A * w / (B - w) - reducedM);
  444.             }
  445.         }

  446.         final double e1 = 1 - e;
  447.         final boolean noCancellationRisk = (e1 + E * E / 6) >= 0.1;

  448.         // perform two iterations, each consisting of one Halley step and one Newton-Raphson step
  449.         for (int j = 0; j < 2; ++j) {
  450.             final double f;
  451.             double fd;
  452.             final double fdd  = e * FastMath.sin(E);
  453.             final double fddd = e * FastMath.cos(E);
  454.             if (noCancellationRisk) {
  455.                 f  = (E - fdd) - reducedM;
  456.                 fd = 1 - fddd;
  457.             } else {
  458.                 f  = eMeSinE(E, e) - reducedM;
  459.                 final double s = FastMath.sin(0.5 * E);
  460.                 fd = e1 + 2 * e * s * s;
  461.             }
  462.             final double dee = f * fd / (0.5 * f * fdd - fd * fd);

  463.             // update eccentric anomaly, using expressions that limit underflow problems
  464.             final double w = fd + 0.5 * dee * (fdd + dee * fddd / 3);
  465.             fd += dee * (fdd + 0.5 * dee * fddd);
  466.             E  -= (f - dee * (fd - w)) / fd;

  467.         }

  468.         // expand the result back to original range
  469.         E += M - reducedM;

  470.         return E;

  471.     }

  472.     /**
  473.      * Accurate computation of E - e sin(E).
  474.      *
  475.      * @param E eccentric anomaly
  476.      * @param e eccentricity
  477.      * @return E - e sin(E)
  478.      */
  479.     private static double eMeSinE(final double E, final double e) {
  480.         double x = (1 - e) * FastMath.sin(E);
  481.         final double mE2 = -E * E;
  482.         double term = E;
  483.         double d    = 0;
  484.         // the inequality test below IS intentional and should NOT be replaced by a check with a small tolerance
  485.         for (double x0 = Double.NaN; !Double.valueOf(x).equals(Double.valueOf(x0));) {
  486.             d += 2;
  487.             term *= mE2 / (d * (d + 1));
  488.             x0 = x;
  489.             x = x - term;
  490.         }
  491.         return x;
  492.     }

  493.     /**
  494.      * Get true anomaly from eccentric anomaly and eccentricity.
  495.      *
  496.      * @param ek the eccentric anomaly (rad)
  497.      * @param ecc the eccentricity
  498.      * @return the true anomaly (rad)
  499.      */
  500.     private double getTrueAnomaly(final double ek, final double ecc) {
  501.         final double svk = FastMath.sqrt(1. - ecc * ecc) * FastMath.sin(ek);
  502.         final double cvk = FastMath.cos(ek) - ecc;
  503.         return FastMath.atan2(svk, cvk);
  504.     }

  505.     /**
  506.      * This method transforms the PV coordinates obtained after the numerical
  507.      * integration in the ECEF PZ-90.
  508.      *
  509.      * @param state spacecraft state after integration
  510.      * @return the PV coordinates in the ECEF PZ-90.
  511.      */
  512.     public PVCoordinates getPVInPZ90(final SpacecraftState state) {

  513.         // Compute time difference between start date and end date
  514.         final double dt = state.getDate().durationFrom(initDate.getDate());

  515.         // Position and velocity vectors
  516.         final PVCoordinates pv = state.getPVCoordinates();
  517.         final Vector3D pos = pv.getPosition();
  518.         final Vector3D vel = pv.getVelocity();

  519.         // Components of position and velocity vectors
  520.         final double x0 = pos.getX();
  521.         final double y0 = pos.getY();
  522.         final double z0 = pos.getZ();
  523.         final double vx0 = vel.getX();
  524.         final double vy0 = vel.getY();
  525.         final double vz0 = vel.getZ();

  526.         // Greenwich Mean Sidereal Time (GMST)
  527.         final GLONASSDate gloDate = new GLONASSDate(
  528.                 state.getDate(),
  529.                 dataContext.getTimeScales().getGLONASS());
  530.         final double gmst = gloDate.getGMST();

  531.         final double ti = glonassOrbit.getTime() + dt;
  532.         // We use the GMST instead of the GMT as it is recommended into GLONASS ICD (2016)
  533.         final double s = gmst + GLONASS_AV * (ti - 10800.);

  534.         // Commons Parameters
  535.         final double cosS = FastMath.cos(s);
  536.         final double sinS = FastMath.sin(s);

  537.         // Transformed coordinates
  538.         final double x = x0 * cosS + y0 * sinS;
  539.         final double y = -x0 * sinS + y0 * cosS;
  540.         final double z = z0;
  541.         final double vx = vx0 * cosS + vy0 * sinS + GLONASS_AV * y;
  542.         final double vy = -vx0 * sinS + vy0 * cosS - GLONASS_AV * x;
  543.         final double vz = vz0;

  544.         // Transformed orbit
  545.         return new PVCoordinates(new Vector3D(x, y, z),
  546.                                  new Vector3D(vx, vy, vz));
  547.     }

  548.     /**
  549.      * This method computes the PV coordinates of the spacecraft center of mass.
  550.      * The returned PV are expressed in inertial coordinates system at the instant tb.
  551.      *
  552.      * @param date the computation date in GLONASS scale
  553.      * @return the PV Coordinates in inertial coordinates system
  554.      */
  555.     private PVCoordinates getPVInInertial(final GLONASSDate date) {

  556.         // Greenwich Mean Sidereal Time (GMST)
  557.         final double gmst = date.getGMST();

  558.         final double time = glonassOrbit.getTime();
  559.         final double dt   = time - 10800.;
  560.         // We use the GMST instead of the GMT as it is recommended into GLONASS ICD (2016)
  561.         final double s = gmst + GLONASS_AV * dt;

  562.         // Commons Parameters
  563.         final double cosS = FastMath.cos(s);
  564.         final double sinS = FastMath.sin(s);

  565.         // PV coordinates in inertial frame
  566.         final double x0  = glonassOrbit.getX() * cosS - glonassOrbit.getY() * sinS;
  567.         final double y0  = glonassOrbit.getX() * sinS + glonassOrbit.getY() * cosS;
  568.         final double z0  = glonassOrbit.getZ();
  569.         final double vx0 = glonassOrbit.getXDot() * cosS - glonassOrbit.getYDot() * sinS - GLONASS_AV * y0;
  570.         final double vy0 = glonassOrbit.getXDot() * sinS + glonassOrbit.getYDot() * cosS + GLONASS_AV * x0;
  571.         final double vz0 = glonassOrbit.getZDot();
  572.         return new PVCoordinates(new Vector3D(x0, y0, z0),
  573.                                  new Vector3D(vx0, vy0, vz0));
  574.     }

  575.     @Override
  576.     protected StateMapper createMapper(final AbsoluteDate referenceDate, final double mu,
  577.                                        final OrbitType orbitType, final PositionAngle positionAngleType,
  578.                                        final AttitudeProvider attitudeProvider, final Frame frame) {
  579.         return new Mapper(referenceDate, mu, orbitType, positionAngleType, attitudeProvider, frame);
  580.     }

  581.     /** Internal mapper. */
  582.     private static class Mapper extends StateMapper {

  583.         /**
  584.          * Simple constructor.
  585.          *
  586.          * @param referenceDate reference date
  587.          * @param mu central attraction coefficient (m³/s²)
  588.          * @param orbitType orbit type to use for mapping
  589.          * @param positionAngleType angle type to use for propagation
  590.          * @param attitudeProvider attitude provider
  591.          * @param frame inertial frame
  592.          */
  593.         Mapper(final AbsoluteDate referenceDate, final double mu,
  594.                final OrbitType orbitType, final PositionAngle positionAngleType,
  595.                final AttitudeProvider attitudeProvider, final Frame frame) {
  596.             super(referenceDate, mu, orbitType, positionAngleType, attitudeProvider, frame);
  597.         }

  598.         @Override
  599.         public SpacecraftState mapArrayToState(final AbsoluteDate date, final double[] y,
  600.                                                final double[] yDot, final PropagationType type) {
  601.             // The parameter meanOnly is ignored for the GLONASS Propagator
  602.             final double mass = y[6];
  603.             if (mass <= 0.0) {
  604.                 throw new OrekitException(OrekitMessages.SPACECRAFT_MASS_BECOMES_NEGATIVE, mass);
  605.             }

  606.             final Orbit orbit       = getOrbitType().mapArrayToOrbit(y, yDot, getPositionAngleType(), date, getMu(), getFrame());
  607.             final Attitude attitude = getAttitudeProvider().getAttitude(orbit, date, getFrame());

  608.             return new SpacecraftState(orbit, attitude, mass);
  609.         }

  610.         @Override
  611.         public void mapStateToArray(final SpacecraftState state, final double[] y,
  612.                                     final double[] yDot) {
  613.             getOrbitType().mapOrbitToArray(state.getOrbit(), getPositionAngleType(), y, yDot);
  614.             y[6] = state.getMass();
  615.         }

  616.     }

  617.     @Override
  618.     protected MainStateEquations getMainStateEquations(final ODEIntegrator integ) {
  619.         return new Main();
  620.     }

  621.     /** Internal class for orbital parameters integration. */
  622.     private class Main implements MainStateEquations {

  623.         /** Derivatives array. */
  624.         private final double[] yDot;

  625.         /**
  626.          * Simple constructor.
  627.          */
  628.         Main() {
  629.             yDot = new double[7];
  630.         }

  631.         @Override
  632.         public double[] computeDerivatives(final SpacecraftState state) {

  633.             // Date in Glonass form
  634.             final GLONASSDate gloDate = new GLONASSDate(
  635.                     state.getDate(),
  636.                     dataContext.getTimeScales().getGLONASS());

  637.             // Position and Velocity vectors
  638.             final Vector3D vel = state.getPVCoordinates().getVelocity();
  639.             final Vector3D pos = state.getPVCoordinates().getPosition();

  640.             Arrays.fill(yDot, 0.0);

  641.             // dPos/dt = Vel
  642.             yDot[0] += vel.getX();
  643.             yDot[1] += vel.getY();
  644.             yDot[2] += vel.getZ();

  645.             // Components of position and velocity vectors
  646.             final double x0 = pos.getX();
  647.             final double y0 = pos.getY();
  648.             final double z0 = pos.getZ();

  649.             // Normalized values
  650.             final double r  = pos.getNorm();
  651.             final double r2 = r * r;
  652.             final double oor = 1. / r;
  653.             final double oor2 = 1. / r2;
  654.             final double x = x0 * oor;
  655.             final double y = y0 * oor;
  656.             final double z = z0 * oor;
  657.             final double g = GLONASSOrbitalElements.GLONASS_MU * oor2;
  658.             final double ro = GLONASS_EARTH_EQUATORIAL_RADIUS * oor;

  659.             yDot[3] += x * (-g + (-1.5 * GLONASS_J20 * g * ro * ro * (1. - 5. * z * z)));
  660.             yDot[4] += y * (-g + (-1.5 * GLONASS_J20 * g * ro * ro * (1. - 5. * z * z)));
  661.             yDot[5] += z * (-g + (-1.5 * GLONASS_J20 * g * ro * ro * (3. - 5. * z * z)));

  662.             // Luni-Solar contribution
  663.             final Vector3D acc;
  664.             if (isAccAvailable) {
  665.                 acc = getLuniSolarPerturbations(gloDate);
  666.             } else {
  667.                 final Vector3D accMoon = computeLuniSolarPerturbations(
  668.                         state, moonElements[0], moonElements[1], moonElements[2],
  669.                         moonElements[3],
  670.                         dataContext.getCelestialBodies().getMoon().getGM());
  671.                 final Vector3D accSun = computeLuniSolarPerturbations(
  672.                         state,
  673.                         sunElements[0], sunElements[1], sunElements[2],
  674.                         sunElements[3],
  675.                         dataContext.getCelestialBodies().getSun().getGM());
  676.                 acc = accMoon.add(accSun);
  677.             }

  678.             yDot[3] += acc.getX();
  679.             yDot[4] += acc.getY();
  680.             yDot[5] += acc.getZ();

  681.             return yDot.clone();
  682.         }

  683.         /**
  684.          * This method computes the accelerations induced by gravitational
  685.          * perturbations of the Sun and the Moon if they are not available into
  686.          * the navigation message data.
  687.          *
  688.          * @param state current state
  689.          * @param eps first direction cosine
  690.          * @param eta second direction cosine
  691.          * @param psi third direction cosine
  692.          * @param r distance of perturbing body
  693.          * @param g body gravitational field constant
  694.          * @return a vector containing the accelerations
  695.          */
  696.         private Vector3D computeLuniSolarPerturbations(final SpacecraftState state, final double eps,
  697.                                                        final double eta, final double psi,
  698.                                                        final double r, final double g) {

  699.             // Current pv coordinates
  700.             final PVCoordinates pv = state.getPVCoordinates();

  701.             final double oor = 1. / r;
  702.             final double oor2 = oor * oor;

  703.             // Normalized variable
  704.             final double x = pv.getPosition().getX() * oor;
  705.             final double y = pv.getPosition().getY() * oor;
  706.             final double z = pv.getPosition().getZ() * oor;
  707.             final double gm = g * oor2;

  708.             final double epsmX  = eps - x;
  709.             final double etamY  = eta - y;
  710.             final double psimZ  = psi - z;
  711.             final Vector3D vector = new Vector3D(epsmX, etamY, psimZ);
  712.             final double d2 = vector.getNormSq();
  713.             final double deltaM = FastMath.sqrt(d2) * d2;

  714.             // Accelerations
  715.             final double accX = gm * ((epsmX / deltaM) - eps);
  716.             final double accY = gm * ((etamY / deltaM) - eta);
  717.             final double accZ = gm * ((psimZ / deltaM) - psi);

  718.             return new Vector3D(accX, accY, accZ);
  719.         }

  720.         /**
  721.          * Get the accelerations induced by gravitational
  722.          * perturbations of the Sun and the Moon in a geocentric
  723.          * coordinate system.
  724.          * <p>
  725.          * The accelerations are obtained using projections of accelerations
  726.          * transmitted within navigation message data.
  727.          * </p>
  728.          *
  729.          * @param date the computation date in GLONASS scale
  730.          * @return a vector containing the sum of both accelerations
  731.          */
  732.         private Vector3D getLuniSolarPerturbations(final GLONASSDate date) {

  733.             // Greenwich Mean Sidereal Time (GMST)
  734.             final double gmst = date.getGMST();

  735.             final double time = glonassOrbit.getTime();
  736.             final double dt   = time - 10800.;
  737.             // We use the GMST instead of the GMT as it is recommended into GLONASS ICD (see Ref)
  738.             final double s = gmst + GLONASS_AV * dt;

  739.             // Commons Parameters
  740.             final double cosS = FastMath.cos(s);
  741.             final double sinS = FastMath.sin(s);

  742.             // Accelerations
  743.             final double accX = glonassOrbit.getXDotDot() * cosS - glonassOrbit.getYDotDot() * sinS;
  744.             final double accY = glonassOrbit.getXDotDot() * sinS + glonassOrbit.getYDotDot() * cosS;
  745.             final double accZ = glonassOrbit.getZDotDot();

  746.             return new Vector3D(accX, accY, accZ);
  747.         }

  748.     }

  749. }