GPSPropagator.java

/* Copyright 2002-2018 CS Systèmes d'Information
 * Licensed to CS Systèmes d'Information (CS) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * CS licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package org.orekit.propagation.analytical.gnss;

import org.hipparchus.analysis.differentiation.DSFactory;
import org.hipparchus.analysis.differentiation.DerivativeStructure;
import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.MathUtils;
import org.hipparchus.util.Precision;
import org.orekit.attitudes.AttitudeProvider;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitMessages;
import org.orekit.frames.Frame;
import org.orekit.frames.FramesFactory;
import org.orekit.orbits.CartesianOrbit;
import org.orekit.orbits.Orbit;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.analytical.AbstractAnalyticalPropagator;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.IERSConventions;
import org.orekit.utils.PVCoordinates;

/**
 * This class aims at propagating a GPS orbit from {@link GPSOrbitalElements}.
 *
 * @see <a href="http://www.gps.gov/technical/icwg/IS-GPS-200H.pdf">GPS Interface Specification</a>
 * @author Pascal Parraud
 * @since 8.0
 */
public class GPSPropagator extends AbstractAnalyticalPropagator {

    // Constants
    /** WGS 84 value of the earth's rotation rate in rad/s. */
    private static final double GPS_AV = 7.2921151467e-5;

    /** Duration of the GPS cycle in seconds. */
    private static final double GPS_CYCLE_DURATION = GPSOrbitalElements.GPS_WEEK_IN_SECONDS *
                                                     GPSOrbitalElements.GPS_WEEK_NB;

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

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

    static {
        final double k1 = 3 * FastMath.PI + 2;
        final double k2 = FastMath.PI - 1;
        final double k3 = 6 * FastMath.PI - 1;
        A  = 3 * k2 * k2 / k1;
        B  = k3 * k3 / (6 * k1);
    }

    // Fields
    /** The GPS orbital elements used. */
    private final GPSOrbitalElements gpsOrbit;

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

    /** The ECI frame used for GPS propagation. */
    private final Frame eci;

    /** The ECEF frame used for GPS propagation. */
    private final Frame ecef;

    /** Factory for the DerivativeStructure instances. */
    private final DSFactory factory;

    /**
     * This nested class aims at building a GPSPropagator.
     * <p>It implements the classical builder pattern.</p>
     *
     */
    public static class Builder {

        // Required parameter
        /** The GPS orbital elements. */
        private final GPSOrbitalElements orbit;

        // Optional parameters
        /** The attitude provider. */
        private AttitudeProvider attitudeProvider = DEFAULT_LAW;
        /** The mass. */
        private double mass = DEFAULT_MASS;
        /** The ECI frame. */
        private Frame eci  = null;
        /** The ECEF frame. */
        private Frame ecef = null;

        /** Initializes the builder.
         * <p>The GPS orbital elements is the only requested parameter to build a GPSPropagator.</p>
         * <p>The attitude provider is set by default to the
         *  {@link org.orekit.propagation.Propagator#DEFAULT_LAW DEFAULT_LAW}.<br>
         * The mass is set by default to the
         *  {@link org.orekit.propagation.Propagator#DEFAULT_MASS DEFAULT_MASS}.<br>
         * The ECI frame is set by default to the
         *  {@link org.orekit.frames.Predefined#EME2000 EME2000 frame}.<br>
         * The ECEF frame is set by default to the
         *  {@link org.orekit.frames.Predefined#ITRF_CIO_CONV_2010_SIMPLE_EOP CIO/2010-based ITRF simple EOP}.
         * </p>
         *
         * @param gpsOrbElt the GPS orbital elements to be used by the GPSpropagator.
         * @throws OrekitException if data embedded in the library cannot be read
         * @see #attitudeProvider(AttitudeProvider provider)
         * @see #mass(double mass)
         * @see #eci(Frame inertial)
         * @see #ecef(Frame bodyFixed)
         */
        public Builder(final GPSOrbitalElements gpsOrbElt) throws OrekitException {
            this.orbit = gpsOrbElt;
            this.eci   = FramesFactory.getEME2000();
            this.ecef  = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
        }

        /** Sets the attitude provider.
         *
         * @param userProvider the attitude provider
         * @return the updated builder
         */
        public Builder attitudeProvider(final AttitudeProvider userProvider) {
            this.attitudeProvider = userProvider;
            return this;
        }

        /** Sets the mass.
         *
         * @param userMass the mass (in kg)
         * @return the updated builder
         */
        public Builder mass(final double userMass) {
            this.mass = userMass;
            return this;
        }

        /** Sets the Earth Centered Inertial frame used for propagation.
         *
         * @param inertial the ECI frame
         * @return the updated builder
         */
        public Builder eci(final Frame inertial) {
            this.eci = inertial;
            return this;
        }

        /** Sets the Earth Centered Earth Fixed frame assimilated to the WGS84 ECEF.
         *
         * @param bodyFixed the ECEF frame
         * @return the updated builder
         */
        public Builder ecef(final Frame bodyFixed) {
            this.ecef = bodyFixed;
            return this;
        }

        /** Finalizes the build.
         *
         * @return the built GPSPropagator
         */
        public GPSPropagator build() {
            return new GPSPropagator(this);
        }
    }

    /**
     * Private constructor.
     *
     * @param builder the builder
     */
    private GPSPropagator(final Builder builder) {
        super(builder.attitudeProvider);
        // Stores the GPS orbital elements
        this.gpsOrbit = builder.orbit;
        // Sets the start date as the date of the orbital elements
        setStartDate(gpsOrbit.getDate());
        // Sets the mass
        this.mass = builder.mass;
        // Sets the Earth Centered Inertial frame
        this.eci  = builder.eci;
        // Sets the Earth Centered Earth Fixed frame
        this.ecef = builder.ecef;

        this.factory = new DSFactory(1, 2);

    }

    /**
     * Gets the PVCoordinates of the GPS SV in {@link #getECEF() ECEF frame}.
     *
     * <p>The algorithm is defined at Table 20-IV from IS-GPS-200 document,
     * with automatic differentiation added to compute velocity and
     * acceleration.</p>
     *
     * @param date the computation date
     * @return the GPS SV PVCoordinates in {@link #getECEF() ECEF frame}
     */
    public PVCoordinates propagateInEcef(final AbsoluteDate date) {
        // Duration from GPS ephemeris Reference date
        final DerivativeStructure tk = factory.variable(0, getTk(date));
        // Mean anomaly
        final DerivativeStructure mk = tk.multiply(gpsOrbit.getMeanMotion()).add(gpsOrbit.getM0());
        // Eccentric Anomaly
        final DerivativeStructure ek = getEccentricAnomaly(mk);
        // True Anomaly
        final DerivativeStructure vk =  getTrueAnomaly(ek);
        // Argument of Latitude
        final DerivativeStructure phik    = vk.add(gpsOrbit.getPa());
        final DerivativeStructure twoPhik = phik.multiply(2);
        final DerivativeStructure c2phi   = twoPhik.cos();
        final DerivativeStructure s2phi   = twoPhik.sin();
        // Argument of Latitude Correction
        final DerivativeStructure dphik = c2phi.multiply(gpsOrbit.getCuc()).add(s2phi.multiply(gpsOrbit.getCus()));
        // Radius Correction
        final DerivativeStructure drk = c2phi.multiply(gpsOrbit.getCrc()).add(s2phi.multiply(gpsOrbit.getCrs()));
        // Inclination Correction
        final DerivativeStructure dik = c2phi.multiply(gpsOrbit.getCic()).add(s2phi.multiply(gpsOrbit.getCis()));
        // Corrected Argument of Latitude
        final DerivativeStructure uk = phik.add(dphik);
        // Corrected Radius
        final DerivativeStructure rk = ek.cos().multiply(-gpsOrbit.getE()).add(1).multiply(gpsOrbit.getSma()).add(drk);
        // Corrected Inclination
        final DerivativeStructure ik  = tk.multiply(gpsOrbit.getIDot()).add(gpsOrbit.getI0()).add(dik);
        final DerivativeStructure cik = ik.cos();
        // Positions in orbital plane
        final DerivativeStructure xk = uk.cos().multiply(rk);
        final DerivativeStructure yk = uk.sin().multiply(rk);
        // Corrected longitude of ascending node
        final DerivativeStructure omk = tk.multiply(gpsOrbit.getOmegaDot() - GPS_AV).
                                        add(gpsOrbit.getOmega0() - GPS_AV * gpsOrbit.getTime());
        final DerivativeStructure comk = omk.cos();
        final DerivativeStructure somk = omk.sin();
        // returns the Earth-fixed coordinates
        final FieldVector3D<DerivativeStructure> positionwithDerivatives =
                        new FieldVector3D<>(xk.multiply(comk).subtract(yk.multiply(somk).multiply(cik)),
                                            xk.multiply(somk).add(yk.multiply(comk).multiply(cik)),
                                            yk.multiply(ik.sin()));
        return new PVCoordinates(positionwithDerivatives);
    }

    /**
     * Get the duration from GPS Reference epoch.
     * <p>This takes the GPS week roll-over into account.</p>
     *
     * @param date the considered date
     * @return the duration from GPS orbit Reference epoch (s)
     */
    private double getTk(final AbsoluteDate date) {
        // Time from ephemeris reference epoch
        double tk = date.durationFrom(gpsOrbit.getDate());
        // Adjusts the time to take roll over week into account
        while (tk > 0.5 * GPS_CYCLE_DURATION) {
            tk -= GPS_CYCLE_DURATION;
        }
        while (tk < -0.5 * GPS_CYCLE_DURATION) {
            tk += GPS_CYCLE_DURATION;
        }
        // Returns the time from ephemeris reference epoch
        return tk;
    }

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

        // reduce M to [-PI PI] interval
        final double[] mlDerivatives = mk.getAllDerivatives();
        mlDerivatives[0] = MathUtils.normalizeAngle(mlDerivatives[0], 0.0);
        final DerivativeStructure reducedM = mk.getFactory().build(mlDerivatives);

        // compute start value according to A. W. Odell and R. H. Gooding S12 starter
        DerivativeStructure ek;
        if (FastMath.abs(reducedM.getValue()) < 1.0 / 6.0) {
            if (FastMath.abs(reducedM.getValue()) < Precision.SAFE_MIN) {
                // this is an Orekit change to the S12 starter.
                // If reducedM is 0.0, the derivative of cbrt is infinite which induces NaN appearing later in
                // the computation. As in this case E and M are almost equal, we initialize ek with reducedM
                ek = reducedM;
            } else {
                // this is the standard S12 starter
                ek = reducedM.add(reducedM.multiply(6).cbrt().subtract(reducedM).multiply(gpsOrbit.getE()));
            }
        } else {
            if (reducedM.getValue() < 0) {
                final DerivativeStructure w = reducedM.add(FastMath.PI);
                ek = reducedM.add(w.multiply(-A).divide(w.subtract(B)).subtract(FastMath.PI).subtract(reducedM).multiply(gpsOrbit.getE()));
            } else {
                final DerivativeStructure minusW = reducedM.subtract(FastMath.PI);
                ek = reducedM.add(minusW.multiply(A).divide(minusW.add(B)).add(FastMath.PI).subtract(reducedM).multiply(gpsOrbit.getE()));
            }
        }

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

        // perform two iterations, each consisting of one Halley step and one Newton-Raphson step
        for (int j = 0; j < 2; ++j) {
            final DerivativeStructure f;
            DerivativeStructure fd;
            final DerivativeStructure fdd  = ek.sin().multiply(gpsOrbit.getE());
            final DerivativeStructure fddd = ek.cos().multiply(gpsOrbit.getE());
            if (noCancellationRisk) {
                f  = ek.subtract(fdd).subtract(reducedM);
                fd = fddd.subtract(1).negate();
            } else {
                f  = eMeSinE(ek).subtract(reducedM);
                final DerivativeStructure s = ek.multiply(0.5).sin();
                fd = s.multiply(s).multiply(2 * gpsOrbit.getE()).add(e1);
            }
            final DerivativeStructure dee = f.multiply(fd).divide(f.multiply(0.5).multiply(fdd).subtract(fd.multiply(fd)));

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

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

        // Returns the eccentric anomaly
        return ek;
    }

    /**
     * Accurate computation of E - e sin(E).
     *
     * @param E eccentric anomaly
     * @return E - e sin(E)
     */
    private DerivativeStructure eMeSinE(final DerivativeStructure E) {
        DerivativeStructure x = E.sin().multiply(1 - gpsOrbit.getE());
        final DerivativeStructure mE2 = E.negate().multiply(E);
        DerivativeStructure term = E;
        DerivativeStructure d    = E.getField().getZero();
        // the inequality test below IS intentional and should NOT be replaced by a check with a small tolerance
        for (DerivativeStructure x0 = d.add(Double.NaN); x.getValue() != x0.getValue();) {
            d = d.add(2);
            term = term.multiply(mE2.divide(d.multiply(d.add(1))));
            x0 = x;
            x = x.subtract(term);
        }
        return x;
    }

    /** Gets true anomaly from eccentric anomaly.
     *
     * @param ek the eccentric anomaly (rad)
     * @return the true anomaly (rad)
     */
    private DerivativeStructure getTrueAnomaly(final DerivativeStructure ek) {
        final DerivativeStructure svk = ek.sin().multiply(FastMath.sqrt(1. - gpsOrbit.getE() * gpsOrbit.getE()));
        final DerivativeStructure cvk = ek.cos().subtract(gpsOrbit.getE());
        return svk.atan2(cvk);
    }

    /**
     * Get the Earth gravity coefficient used for GPS propagation.
     * @return the Earth gravity coefficient.
     */
    public static double getMU() {
        return GPSOrbitalElements.GPS_MU;
    }

    /**
     * Gets the underlying GPS orbital elements.
     *
     * @return the underlying GPS orbital elements
     */
    public GPSOrbitalElements getGPSOrbitalElements() {
        return gpsOrbit;
    }

    /**
     * Gets the Earth Centered Inertial frame used to propagate the orbit.
     *
     * @return the ECI frame
     */
    public Frame getECI() {
        return eci;
    }

    /**
     * Gets the Earth Centered Earth Fixed frame used to propagate GPS orbits according to the
     * <a href="http://www.gps.gov/technical/icwg/IS-GPS-200H.pdf">GPS Interface Specification</a>.
     * <p>This frame is assimilated to the WGS84 ECEF.</p>
     *
     * @return the ECEF frame
     */
    public Frame getECEF() {
        return ecef;
    }

    /** {@inheritDoc} */
    public Frame getFrame() {
        return eci;
    }

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

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

    /** {@inheritDoc} */
    protected double getMass(final AbsoluteDate date) {
        return mass;
    }

    /** {@inheritDoc} */
    protected Orbit propagateOrbit(final AbsoluteDate date) throws OrekitException {
        // Gets the PVCoordinates in ECEF frame
        final PVCoordinates pvaInECEF = propagateInEcef(date);
        // Transforms the PVCoordinates to ECI frame
        final PVCoordinates pvaInECI = ecef.getTransformTo(eci, date).transformPVCoordinates(pvaInECEF);
        // Returns the Cartesian orbit
        return new CartesianOrbit(pvaInECI, eci, date, GPSOrbitalElements.GPS_MU);
    }
}