GNSSPropagator.java

  1. /* Copyright 2002-2024 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.Attitude;
  25. import org.orekit.attitudes.AttitudeProvider;
  26. import org.orekit.errors.OrekitException;
  27. import org.orekit.errors.OrekitMessages;
  28. import org.orekit.frames.Frame;
  29. import org.orekit.orbits.CartesianOrbit;
  30. import org.orekit.orbits.Orbit;
  31. import org.orekit.propagation.SpacecraftState;
  32. import org.orekit.propagation.analytical.AbstractAnalyticalPropagator;
  33. import org.orekit.propagation.analytical.gnss.data.GNSSOrbitalElements;
  34. import org.orekit.time.AbsoluteDate;
  35. import org.orekit.utils.PVCoordinates;

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

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

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

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

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

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

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

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

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

  91.     /**
  92.      * Gets the Earth Centered Inertial frame used to propagate the orbit.
  93.      *
  94.      * @return the ECI frame
  95.      */
  96.     public Frame getECI() {
  97.         return eci;
  98.     }

  99.     /**
  100.      * Gets the Earth Centered Earth Fixed frame used to propagate GNSS orbits according to the
  101.      * Interface Control Document.
  102.      *
  103.      * @return the ECEF frame
  104.      */
  105.     public Frame getECEF() {
  106.         return ecef;
  107.     }

  108.     /**
  109.      * Gets the Earth gravity coefficient used for GNSS propagation.
  110.      *
  111.      * @return the Earth gravity coefficient.
  112.      */
  113.     public double getMU() {
  114.         return gnssOrbit.getMu();
  115.     }

  116.     /**
  117.      * Get the underlying GNSS orbital elements.
  118.      *
  119.      * @return the underlying GNSS orbital elements
  120.      */
  121.     public GNSSOrbitalElements getOrbitalElements() {
  122.         return gnssOrbit;
  123.     }

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

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

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

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

  224.         // reduce M to [-PI PI] interval
  225.         final UnivariateDerivative2 reducedM = new UnivariateDerivative2(MathUtils.normalizeAngle(mk.getValue(), 0.0),
  226.                                                                          mk.getFirstDerivative(),
  227.                                                                          mk.getSecondDerivative());

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

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

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

  266.             // update eccentric anomaly, using expressions that limit underflow problems
  267.             final UnivariateDerivative2 w = fd.add(dee.multiply(0.5).multiply(fdd.add(dee.multiply(fdd).divide(3))));
  268.             fd = fd.add(dee.multiply(fdd.add(dee.multiply(0.5).multiply(fdd))));
  269.             ek = ek.subtract(f.subtract(dee.multiply(fd.subtract(w))).divide(fd));
  270.         }

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

  273.         // Returns the eccentric anomaly
  274.         return ek;
  275.     }

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

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

  306.     /** {@inheritDoc} */
  307.     @Override
  308.     public Frame getFrame() {
  309.         return eci;
  310.     }

  311.     /** {@inheritDoc} */
  312.     @Override
  313.     protected double getMass(final AbsoluteDate date) {
  314.         return mass;
  315.     }

  316.     /** {@inheritDoc} */
  317.     @Override
  318.     public void resetInitialState(final SpacecraftState state) {
  319.         throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE);
  320.     }

  321.     /** {@inheritDoc} */
  322.     @Override
  323.     protected void resetIntermediateState(final SpacecraftState state, final boolean forward) {
  324.         throw new OrekitException(OrekitMessages.NON_RESETABLE_STATE);
  325.     }

  326. }