GNSSPropagator.java

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

  18. import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
  19. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  20. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  21. import org.hipparchus.util.FastMath;
  22. import org.hipparchus.util.MathUtils;
  23. import org.hipparchus.util.Precision;
  24. import org.orekit.attitudes.AttitudeProvider;
  25. import org.orekit.errors.OrekitException;
  26. import org.orekit.errors.OrekitMessages;
  27. import org.orekit.frames.Frame;
  28. import org.orekit.orbits.CartesianOrbit;
  29. import org.orekit.orbits.Orbit;
  30. import org.orekit.propagation.SpacecraftState;
  31. import org.orekit.propagation.analytical.AbstractAnalyticalPropagator;
  32. import org.orekit.propagation.analytical.gnss.data.GNSSOrbitalElements;
  33. import org.orekit.time.AbsoluteDate;
  34. import org.orekit.utils.PVCoordinates;

  35. /** Common handling of {@link AbstractAnalyticalPropagator} methods for GNSS propagators.
  36.  * <p>
  37.  * This class allows to provide easily a subset of {@link AbstractAnalyticalPropagator} methods
  38.  * for specific GNSS propagators.
  39.  * </p>
  40.  * @author Pascal Parraud
  41.  */
  42. public class GNSSPropagator extends AbstractAnalyticalPropagator {

  43.     // Data used to solve Kepler's equation
  44.     /** First coefficient to compute Kepler equation solver starter. */
  45.     private static final double A;

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

  48.     static {
  49.         final double k1 = 3 * FastMath.PI + 2;
  50.         final double k2 = FastMath.PI - 1;
  51.         final double k3 = 6 * FastMath.PI - 1;
  52.         A  = 3 * k2 * k2 / k1;
  53.         B  = k3 * k3 / (6 * k1);
  54.     }

  55.     /** The GNSS orbital elements used. */
  56.     private final GNSSOrbitalElements gnssOrbit;

  57.     /** The spacecraft mass (kg). */
  58.     private final double mass;

  59.     /** The ECI frame used for GNSS propagation. */
  60.     private final Frame eci;

  61.     /** The ECEF frame used for GNSS propagation. */
  62.     private final Frame ecef;

  63.     /**
  64.      * Build a new instance.
  65.      * @param gnssOrbit GNSS orbital elements
  66.      * @param eci Earth Centered Inertial frame
  67.      * @param ecef Earth Centered Earth Fixed frame
  68.      * @param provider Attitude provider
  69.      * @param mass Satellite mass (kg)
  70.      */
  71.     GNSSPropagator(final GNSSOrbitalElements gnssOrbit, final Frame eci,
  72.                    final Frame ecef, final AttitudeProvider provider,
  73.                    final double mass) {
  74.         super(provider);
  75.         // Stores the GNSS orbital elements
  76.         this.gnssOrbit = gnssOrbit;
  77.         // Sets the start date as the date of the orbital elements
  78.         setStartDate(gnssOrbit.getDate());
  79.         // Sets the mass
  80.         this.mass = mass;
  81.         // Sets the Earth Centered Inertial frame
  82.         this.eci  = eci;
  83.         // Sets the Earth Centered Earth Fixed frame
  84.         this.ecef = ecef;
  85.     }

  86.     /**
  87.      * Gets the Earth Centered Inertial frame used to propagate the orbit.
  88.      *
  89.      * @return the ECI frame
  90.      */
  91.     public Frame getECI() {
  92.         return eci;
  93.     }

  94.     /**
  95.      * Gets the Earth Centered Earth Fixed frame used to propagate GNSS orbits according to the
  96.      * Interface Control Document.
  97.      *
  98.      * @return the ECEF frame
  99.      */
  100.     public Frame getECEF() {
  101.         return ecef;
  102.     }

  103.     /**
  104.      * Gets the Earth gravity coefficient used for GNSS propagation.
  105.      *
  106.      * @return the Earth gravity coefficient.
  107.      */
  108.     public double getMU() {
  109.         return gnssOrbit.getMu();
  110.     }

  111.     /**
  112.      * Get the underlying GNSS orbital elements.
  113.      *
  114.      * @return the underlying GNSS orbital elements
  115.      */
  116.     public GNSSOrbitalElements getOrbitalElements() {
  117.         return gnssOrbit;
  118.     }

  119.     /** {@inheritDoc} */
  120.     @Override
  121.     protected Orbit propagateOrbit(final AbsoluteDate date) {
  122.         // Gets the PVCoordinates in ECEF frame
  123.         final PVCoordinates pvaInECEF = propagateInEcef(date);
  124.         // Transforms the PVCoordinates to ECI frame
  125.         final PVCoordinates pvaInECI = ecef.getTransformTo(eci, date).transformPVCoordinates(pvaInECEF);
  126.         // Returns the Cartesian orbit
  127.         return new CartesianOrbit(pvaInECI, eci, date, getMU());
  128.     }

  129.     /**
  130.      * Gets the PVCoordinates of the GNSS SV in {@link #getECEF() ECEF frame}.
  131.      *
  132.      * <p>The algorithm uses automatic differentiation to compute velocity and
  133.      * acceleration.</p>
  134.      *
  135.      * @param date the computation date
  136.      * @return the GNSS SV PVCoordinates in {@link #getECEF() ECEF frame}
  137.      */
  138.     public PVCoordinates propagateInEcef(final AbsoluteDate date) {
  139.         // Duration from GNSS ephemeris Reference date
  140.         final UnivariateDerivative2 tk = new UnivariateDerivative2(getTk(date), 1.0, 0.0);
  141.         // Mean anomaly
  142.         final UnivariateDerivative2 mk = tk.multiply(gnssOrbit.getMeanMotion()).add(gnssOrbit.getM0());
  143.         // Eccentric Anomaly
  144.         final UnivariateDerivative2 ek = getEccentricAnomaly(mk);
  145.         // True Anomaly
  146.         final UnivariateDerivative2 vk =  getTrueAnomaly(ek);
  147.         // Argument of Latitude
  148.         final UnivariateDerivative2 phik    = vk.add(gnssOrbit.getPa());
  149.         final UnivariateDerivative2 twoPhik = phik.multiply(2);
  150.         final UnivariateDerivative2 c2phi   = twoPhik.cos();
  151.         final UnivariateDerivative2 s2phi   = twoPhik.sin();
  152.         // Argument of Latitude Correction
  153.         final UnivariateDerivative2 dphik = c2phi.multiply(gnssOrbit.getCuc()).add(s2phi.multiply(gnssOrbit.getCus()));
  154.         // Radius Correction
  155.         final UnivariateDerivative2 drk = c2phi.multiply(gnssOrbit.getCrc()).add(s2phi.multiply(gnssOrbit.getCrs()));
  156.         // Inclination Correction
  157.         final UnivariateDerivative2 dik = c2phi.multiply(gnssOrbit.getCic()).add(s2phi.multiply(gnssOrbit.getCis()));
  158.         // Corrected Argument of Latitude
  159.         final UnivariateDerivative2 uk = phik.add(dphik);
  160.         // Corrected Radius
  161.         final UnivariateDerivative2 rk = ek.cos().multiply(-gnssOrbit.getE()).add(1).multiply(gnssOrbit.getSma()).add(drk);
  162.         // Corrected Inclination
  163.         final UnivariateDerivative2 ik  = tk.multiply(gnssOrbit.getIDot()).add(gnssOrbit.getI0()).add(dik);
  164.         final UnivariateDerivative2 cik = ik.cos();
  165.         // Positions in orbital plane
  166.         final UnivariateDerivative2 xk = uk.cos().multiply(rk);
  167.         final UnivariateDerivative2 yk = uk.sin().multiply(rk);
  168.         // Corrected longitude of ascending node
  169.         final UnivariateDerivative2 omk = tk.multiply(gnssOrbit.getOmegaDot() - gnssOrbit.getAngularVelocity()).
  170.                                         add(gnssOrbit.getOmega0() - gnssOrbit.getAngularVelocity() * gnssOrbit.getTime());
  171.         final UnivariateDerivative2 comk = omk.cos();
  172.         final UnivariateDerivative2 somk = omk.sin();
  173.         // returns the Earth-fixed coordinates
  174.         final FieldVector3D<UnivariateDerivative2> positionwithDerivatives =
  175.                         new FieldVector3D<>(xk.multiply(comk).subtract(yk.multiply(somk).multiply(cik)),
  176.                                             xk.multiply(somk).add(yk.multiply(comk).multiply(cik)),
  177.                                             yk.multiply(ik.sin()));
  178.         return new PVCoordinates(new Vector3D(positionwithDerivatives.getX().getValue(),
  179.                                               positionwithDerivatives.getY().getValue(),
  180.                                               positionwithDerivatives.getZ().getValue()),
  181.                                  new Vector3D(positionwithDerivatives.getX().getFirstDerivative(),
  182.                                               positionwithDerivatives.getY().getFirstDerivative(),
  183.                                               positionwithDerivatives.getZ().getFirstDerivative()),
  184.                                  new Vector3D(positionwithDerivatives.getX().getSecondDerivative(),
  185.                                               positionwithDerivatives.getY().getSecondDerivative(),
  186.                                               positionwithDerivatives.getZ().getSecondDerivative()));
  187.     }

  188.     /**
  189.      * Gets the duration from GNSS Reference epoch.
  190.      * <p>This takes the GNSS week roll-over into account.</p>
  191.      * @param date the considered date
  192.      * @return the duration from GNSS orbit Reference epoch (s)
  193.      */
  194.     private double getTk(final AbsoluteDate date) {
  195.         final double cycleDuration = gnssOrbit.getCycleDuration();
  196.         // Time from ephemeris reference epoch
  197.         double tk = date.durationFrom(gnssOrbit.getDate());
  198.         // Adjusts the time to take roll over week into account
  199.         while (tk > 0.5 * cycleDuration) {
  200.             tk -= cycleDuration;
  201.         }
  202.         while (tk < -0.5 * cycleDuration) {
  203.             tk += cycleDuration;
  204.         }
  205.         // Returns the time from ephemeris reference epoch
  206.         return tk;
  207.     }

  208.     /**
  209.      * Gets eccentric anomaly from mean anomaly.
  210.      * <p>The algorithm used to solve the Kepler equation has been published in:
  211.      * "Procedures for  solving Kepler's Equation", A. W. Odell and R. H. Gooding,
  212.      * Celestial Mechanics 38 (1986) 307-334</p>
  213.      * <p>It has been copied from the OREKIT library (KeplerianOrbit class).</p>
  214.      *
  215.      * @param mk the mean anomaly (rad)
  216.      * @return the eccentric anomaly (rad)
  217.      */
  218.     private UnivariateDerivative2 getEccentricAnomaly(final UnivariateDerivative2 mk) {

  219.         // reduce M to [-PI PI] interval
  220.         final UnivariateDerivative2 reducedM = new UnivariateDerivative2(MathUtils.normalizeAngle(mk.getValue(), 0.0),
  221.                                                                          mk.getFirstDerivative(),
  222.                                                                          mk.getSecondDerivative());

  223.         // compute start value according to A. W. Odell and R. H. Gooding S12 starter
  224.         UnivariateDerivative2 ek;
  225.         if (FastMath.abs(reducedM.getValue()) < 1.0 / 6.0) {
  226.             if (FastMath.abs(reducedM.getValue()) < Precision.SAFE_MIN) {
  227.                 // this is an Orekit change to the S12 starter.
  228.                 // If reducedM is 0.0, the derivative of cbrt is infinite which induces NaN appearing later in
  229.                 // the computation. As in this case E and M are almost equal, we initialize ek with reducedM
  230.                 ek = reducedM;
  231.             } else {
  232.                 // this is the standard S12 starter
  233.                 ek = reducedM.add(reducedM.multiply(6).cbrt().subtract(reducedM).multiply(gnssOrbit.getE()));
  234.             }
  235.         } else {
  236.             if (reducedM.getValue() < 0) {
  237.                 final UnivariateDerivative2 w = reducedM.add(FastMath.PI);
  238.                 ek = reducedM.add(w.multiply(-A).divide(w.subtract(B)).subtract(FastMath.PI).subtract(reducedM).multiply(gnssOrbit.getE()));
  239.             } else {
  240.                 final UnivariateDerivative2 minusW = reducedM.subtract(FastMath.PI);
  241.                 ek = reducedM.add(minusW.multiply(A).divide(minusW.add(B)).add(FastMath.PI).subtract(reducedM).multiply(gnssOrbit.getE()));
  242.             }
  243.         }

  244.         final double e1 = 1 - gnssOrbit.getE();
  245.         final boolean noCancellationRisk = (e1 + ek.getValue() * ek.getValue() / 6) >= 0.1;

  246.         // perform two iterations, each consisting of one Halley step and one Newton-Raphson step
  247.         for (int j = 0; j < 2; ++j) {
  248.             final UnivariateDerivative2 f;
  249.             UnivariateDerivative2 fd;
  250.             final UnivariateDerivative2 fdd  = ek.sin().multiply(gnssOrbit.getE());
  251.             final UnivariateDerivative2 fddd = ek.cos().multiply(gnssOrbit.getE());
  252.             if (noCancellationRisk) {
  253.                 f  = ek.subtract(fdd).subtract(reducedM);
  254.                 fd = fddd.subtract(1).negate();
  255.             } else {
  256.                 f  = eMeSinE(ek).subtract(reducedM);
  257.                 final UnivariateDerivative2 s = ek.multiply(0.5).sin();
  258.                 fd = s.multiply(s).multiply(2 * gnssOrbit.getE()).add(e1);
  259.             }
  260.             final UnivariateDerivative2 dee = f.multiply(fd).divide(f.multiply(0.5).multiply(fdd).subtract(fd.multiply(fd)));

  261.             // update eccentric anomaly, using expressions that limit underflow problems
  262.             final UnivariateDerivative2 w = fd.add(dee.multiply(0.5).multiply(fdd.add(dee.multiply(fdd).divide(3))));
  263.             fd = fd.add(dee.multiply(fdd.add(dee.multiply(0.5).multiply(fdd))));
  264.             ek = ek.subtract(f.subtract(dee.multiply(fd.subtract(w))).divide(fd));
  265.         }

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

  268.         // Returns the eccentric anomaly
  269.         return ek;
  270.     }

  271.     /**
  272.      * Accurate computation of E - e sin(E).
  273.      *
  274.      * @param E eccentric anomaly
  275.      * @return E - e sin(E)
  276.      */
  277.     private UnivariateDerivative2 eMeSinE(final UnivariateDerivative2 E) {
  278.         UnivariateDerivative2 x = E.sin().multiply(1 - gnssOrbit.getE());
  279.         final UnivariateDerivative2 mE2 = E.negate().multiply(E);
  280.         UnivariateDerivative2 term = E;
  281.         UnivariateDerivative2 d    = E.getField().getZero();
  282.         // the inequality test below IS intentional and should NOT be replaced by a check with a small tolerance
  283.         for (UnivariateDerivative2 x0 = d.add(Double.NaN); !Double.valueOf(x.getValue()).equals(Double.valueOf(x0.getValue()));) {
  284.             d = d.add(2);
  285.             term = term.multiply(mE2.divide(d.multiply(d.add(1))));
  286.             x0 = x;
  287.             x = x.subtract(term);
  288.         }
  289.         return x;
  290.     }

  291.     /** Gets true anomaly from eccentric anomaly.
  292.      *
  293.      * @param ek the eccentric anomaly (rad)
  294.      * @return the true anomaly (rad)
  295.      */
  296.     private UnivariateDerivative2 getTrueAnomaly(final UnivariateDerivative2 ek) {
  297.         final UnivariateDerivative2 svk = ek.sin().multiply(FastMath.sqrt(1. - gnssOrbit.getE() * gnssOrbit.getE()));
  298.         final UnivariateDerivative2 cvk = ek.cos().subtract(gnssOrbit.getE());
  299.         return svk.atan2(cvk);
  300.     }

  301.     /** {@inheritDoc} */
  302.     @Override
  303.     public Frame getFrame() {
  304.         return eci;
  305.     }

  306.     /** {@inheritDoc} */
  307.     @Override
  308.     protected double getMass(final AbsoluteDate date) {
  309.         return mass;
  310.     }

  311.     /** {@inheritDoc} */
  312.     @Override
  313.     public void resetInitialState(final SpacecraftState state) {
  314.         throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE);
  315.     }

  316.     /** {@inheritDoc} */
  317.     @Override
  318.     protected void resetIntermediateState(final SpacecraftState state, final boolean forward) {
  319.         throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE);
  320.     }

  321. }