FieldGnssPropagator.java

  1. /* Copyright 2022-2025 Luc Maisonobe
  2.  * Licensed to CS GROUP (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.propagation.analytical.gnss;

  19. import org.hipparchus.CalculusFieldElement;
  20. import org.hipparchus.Field;
  21. import org.hipparchus.analysis.differentiation.FieldGradient;
  22. import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2;
  23. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  24. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  25. import org.hipparchus.linear.FieldMatrix;
  26. import org.hipparchus.linear.FieldQRDecomposition;
  27. import org.hipparchus.linear.FieldVector;
  28. import org.hipparchus.linear.MatrixUtils;
  29. import org.hipparchus.util.FastMath;
  30. import org.hipparchus.util.FieldSinCos;
  31. import org.hipparchus.util.MathArrays;
  32. import org.orekit.attitudes.AttitudeProvider;
  33. import org.orekit.attitudes.FieldAttitude;
  34. import org.orekit.frames.Frame;
  35. import org.orekit.orbits.FieldCartesianOrbit;
  36. import org.orekit.orbits.FieldKeplerianAnomalyUtility;
  37. import org.orekit.orbits.FieldKeplerianOrbit;
  38. import org.orekit.orbits.FieldOrbit;
  39. import org.orekit.orbits.PositionAngleType;
  40. import org.orekit.propagation.FieldSpacecraftState;
  41. import org.orekit.propagation.analytical.FieldAbstractAnalyticalPropagator;
  42. import org.orekit.propagation.analytical.gnss.data.FieldGnssOrbitalElements;
  43. import org.orekit.propagation.analytical.gnss.data.GNSSOrbitalElements;
  44. import org.orekit.time.FieldAbsoluteDate;
  45. import org.orekit.utils.FieldPVCoordinates;
  46. import org.orekit.utils.ParameterDriver;

  48. import java.util.List;

  50. /** Common handling of {@link FieldAbstractAnalyticalPropagator} methods for GNSS propagators.
  51.  * <p>
  52.  * This class allows to provide easily a subset of {@link FieldAbstractAnalyticalPropagator} methods
  53.  * for specific GNSS propagators.
  54.  * </p>
  55.  * @author Pascal Parraud
  56.  * @author Luc Maisonobe
  57.  * @param <T> type of the field elements
  58.  * @since 13.0
  59.  */
  60. public class FieldGnssPropagator<T extends CalculusFieldElement<T>> extends FieldAbstractAnalyticalPropagator<T> {

  62.     /** Maximum number of iterations for internal loops. */
  63.     private static final int MAX_ITER = 100;

  65.     /** Tolerance on position for rebuilding orbital elements from initial state. */
  66.     private static final double TOL_P = 1.0e-6;

  68.     /** Tolerance on velocity for rebuilding orbital elements from initial state. */
  69.     private static final double TOL_V = 1.0e-9;

  71.     /** Number of free parameters for orbital elements. */
  72.     private static final int FREE_PARAMETERS = 6;

  74.     /** Convergence parameter. */
  75.     private static final double EPS = 1.0e-12;

  77.     /** The GNSS propagation model used. */
  78.     private FieldGnssOrbitalElements<T, ?> orbitalElements;

  80.     /** The ECI frame used for GNSS propagation. */
  81.     private final Frame eci;

  83.     /** The ECEF frame used for GNSS propagation. */
  84.     private final Frame ecef;

  86.     /**
  87.      * Build a new instance.
  88.      * @param orbitalElements GNSS orbital elements
  89.      * @param eci Earth Centered Inertial frame
  90.      * @param ecef Earth Centered Earth Fixed frame
  91.      * @param provider Attitude provider
  92.      * @param mass Satellite mass (kg)
  93.      */
  94.     FieldGnssPropagator(final FieldGnssOrbitalElements<T, ?> orbitalElements,
  95.                         final Frame eci, final Frame ecef,
  96.                         final AttitudeProvider provider, final T mass) {
  97.         super(orbitalElements.getDate().getField(), provider);
  98.         // Stores the GNSS orbital elements
  99.         this.orbitalElements = orbitalElements;
  100.        // Sets the Earth Centered Inertial frame
  101.         this.eci  = eci;
  102.         // Sets the Earth Centered Earth Fixed frame
  103.         this.ecef = ecef;
  104.         // Sets initial state
  105.         final FieldOrbit<T> orbit = propagateOrbit(orbitalElements.getDate(), defaultParameters());
  106.         final FieldAttitude<T> attitude = provider.getAttitude(orbit, orbit.getDate(), orbit.getFrame());

  108.         // calling the method from base class because the one overridden below recomputes the orbital elements
  109.         super.resetInitialState(new FieldSpacecraftState<>(orbit, attitude).withMass(mass));

  111.     }

  113.     /**
  114.      * Build a new instance from an initial state.
  115.      * <p>
  116.      * The Keplerian elements already present in the {@code nonKeplerianElements} argument
  117.      * will be ignored as it is the {@code initialState} argument that will be used to
  118.      * build the complete orbital elements of the propagator
  119.      * </p>
  120.      * @param initialState         initial state
  121.      * @param nonKeplerianElements non-Keplerian orbital elements (the Keplerian orbital elements will be ignored)
  122.      * @param ecef                 Earth Centered Earth Fixed frame
  123.      * @param provider             attitude provider
  124.      * @param mass                 spacecraft mass
  125.      */
  126.     FieldGnssPropagator(final FieldSpacecraftState<T> initialState,
  127.                         final FieldGnssOrbitalElements<T, ?> nonKeplerianElements,
  128.                         final Frame ecef, final AttitudeProvider provider, final T mass) {
  129.         this(buildOrbitalElements(initialState, nonKeplerianElements, ecef, provider, mass),
  130.              initialState.getFrame(), ecef, provider, initialState.getMass());
  131.     }

  133.     /** Build default parameters.
  134.      * @return default parameters
  135.      */
  136.     private T[] defaultParameters() {
  137.         final T[] parameters = MathArrays.buildArray(orbitalElements.getDate().getField(), GNSSOrbitalElements.SIZE);
  138.         parameters[GNSSOrbitalElements.TIME_INDEX]      = getMU().newInstance(orbitalElements.getTime());
  139.         parameters[GNSSOrbitalElements.I_DOT_INDEX]     = getMU().newInstance(orbitalElements.getIDot());
  140.         parameters[GNSSOrbitalElements.OMEGA_DOT_INDEX] = getMU().newInstance(orbitalElements.getOmegaDot());
  141.         parameters[GNSSOrbitalElements.CUC_INDEX]       = getMU().newInstance(orbitalElements.getCuc());
  142.         parameters[GNSSOrbitalElements.CUS_INDEX]       = getMU().newInstance(orbitalElements.getCus());
  143.         parameters[GNSSOrbitalElements.CRC_INDEX]       = getMU().newInstance(orbitalElements.getCrc());
  144.         parameters[GNSSOrbitalElements.CRS_INDEX]       = getMU().newInstance(orbitalElements.getCrs());
  145.         parameters[GNSSOrbitalElements.CIC_INDEX]       = getMU().newInstance(orbitalElements.getCic());
  146.         parameters[GNSSOrbitalElements.CIS_INDEX]       = getMU().newInstance(orbitalElements.getCis());
  147.         return parameters;
  148.     }

  150.     /** {@inheritDoc} */
  151.     @Override
  152.     public List<ParameterDriver> getParametersDrivers() {
  153.         return orbitalElements.getParametersDrivers();
  154.     }

  156.     /**
  157.      * Gets the Earth Centered Inertial frame used to propagate the orbit.
  158.      *
  159.      * @return the ECI frame
  160.      */
  161.     public Frame getECI() {
  162.         return eci;
  163.     }

  165.     /**
  166.      * Gets the Earth Centered Earth Fixed frame used to propagate GNSS orbits according to the
  167.      * Interface Control Document.
  168.      *
  169.      * @return the ECEF frame
  170.      */
  171.     public Frame getECEF() {
  172.         return ecef;
  173.     }

  175.     /**
  176.      * Gets the Earth gravity coefficient used for GNSS propagation.
  177.      *
  178.      * @return the Earth gravity coefficient.
  179.      */
  180.     public T getMU() {
  181.         return orbitalElements.getMu();
  182.     }

  184.     /** {@inheritDoc} */
  185.     @Override
  186.     public FieldOrbit<T> propagateOrbit(final FieldAbsoluteDate<T> date,
  187.                                         final T[] parameters) {
  188.         // Gets the PVCoordinates in ECEF frame
  189.         final FieldPVCoordinates<T> pvaInECEF = propagateInEcef(date, parameters);
  190.         // Transforms the PVCoordinates to ECI frame
  191.         final FieldPVCoordinates<T> pvaInECI = ecef.getTransformTo(eci, date).transformPVCoordinates(pvaInECEF);
  192.         // Returns the Cartesian orbit
  193.         return new FieldCartesianOrbit<>(pvaInECI, eci, date, getMU());
  194.     }

  196.     /**
  197.      * Gets the PVCoordinates of the GNSS SV in {@link #getECEF() ECEF frame}.
  198.      *
  199.      * <p>The algorithm uses automatic differentiation to compute velocity and
  200.      * acceleration.</p>
  201.      *
  202.      * @param date the computation date
  203.      * @param parameters propagation parameters
  204.      * @return the GNSS SV PVCoordinates in {@link #getECEF() ECEF frame}
  205.      */
  206.     public FieldPVCoordinates<T> propagateInEcef(final FieldAbsoluteDate<T> date, final T[] parameters) {
  207.         // Duration from GNSS ephemeris Reference date
  208.         final FieldUnivariateDerivative2<T> tk = new FieldUnivariateDerivative2<>(getTk(date),
  209.                                                                                   date.getField().getOne(),
  210.                                                                                   date.getField().getZero());

  212.         // Semi-major axis
  213.         final FieldUnivariateDerivative2<T> ak = tk.multiply(orbitalElements.getADot()).add(orbitalElements.getSma());
  214.         // Mean motion
  215.         final FieldUnivariateDerivative2<T> nA = tk.multiply(orbitalElements.getDeltaN0Dot().multiply(0.5)).
  216.                                                  add(orbitalElements.getDeltaN0()).
  217.                                                  add(orbitalElements.getMeanMotion0());
  218.         // Mean anomaly
  219.         final FieldUnivariateDerivative2<T> mk = tk.multiply(nA).add(orbitalElements.getM0());
  220.         // Eccentric Anomaly
  221.         final FieldUnivariateDerivative2<T> e  = tk.newInstance(orbitalElements.getE());
  222.         final FieldUnivariateDerivative2<T> ek = FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(e, mk);
  223.         // True Anomaly
  224.         final FieldUnivariateDerivative2<T> vk = FieldKeplerianAnomalyUtility.ellipticEccentricToTrue(e, ek);
  225.         // Argument of Latitude
  226.         final FieldUnivariateDerivative2<T> phik    = vk.add(orbitalElements.getPa());
  227.         final FieldSinCos<FieldUnivariateDerivative2<T>> cs2phi = FastMath.sinCos(phik.multiply(2));
  228.         // Argument of Latitude Correction
  229.         final FieldUnivariateDerivative2<T> dphik = cs2phi.cos().multiply(parameters[GNSSOrbitalElements.CUC_INDEX]).
  230.                                                 add(cs2phi.sin().multiply(parameters[GNSSOrbitalElements.CUS_INDEX]));
  231.         // Radius Correction
  232.         final FieldUnivariateDerivative2<T> drk = cs2phi.cos().multiply(parameters[GNSSOrbitalElements.CRC_INDEX]).
  233.                                               add(cs2phi.sin().multiply(parameters[GNSSOrbitalElements.CRS_INDEX]));
  234.         // Inclination Correction
  235.         final FieldUnivariateDerivative2<T> dik = cs2phi.cos().multiply(parameters[GNSSOrbitalElements.CIC_INDEX]).
  236.                                               add(cs2phi.sin().multiply(parameters[GNSSOrbitalElements.CIS_INDEX]));
  237.         // Corrected Argument of Latitude
  238.         final FieldSinCos<FieldUnivariateDerivative2<T>> csuk = FastMath.sinCos(phik.add(dphik));
  239.         // Corrected Radius
  240.         final FieldUnivariateDerivative2<T> rk = ek.cos().multiply(e.negate()).add(1).multiply(ak).add(drk);
  241.         // Corrected Inclination
  242.         final FieldUnivariateDerivative2<T> ik  = tk.multiply(parameters[GNSSOrbitalElements.I_DOT_INDEX]).
  243.                                                   add(orbitalElements.getI0()).add(dik);
  244.         final FieldSinCos<FieldUnivariateDerivative2<T>> csik = FastMath.sinCos(ik);
  245.         // Positions in orbital plane
  246.         final FieldUnivariateDerivative2<T> xk = csuk.cos().multiply(rk);
  247.         final FieldUnivariateDerivative2<T> yk = csuk.sin().multiply(rk);
  248.         // Corrected longitude of ascending node
  249.         final FieldSinCos<FieldUnivariateDerivative2<T>> csomk =
  250.             FastMath.sinCos(tk.multiply(parameters[GNSSOrbitalElements.OMEGA_DOT_INDEX].
  251.                             subtract(orbitalElements.getAngularVelocity())).
  252.                             add(orbitalElements.getOmega0()).
  253.                             subtract(parameters[GNSSOrbitalElements.TIME_INDEX].multiply(orbitalElements.getAngularVelocity())));
  254.         // returns the Earth-fixed coordinates
  255.         final FieldVector3D<FieldUnivariateDerivative2<T>> positionWithDerivatives =
  256.                         new FieldVector3D<>(xk.multiply(csomk.cos()).subtract(yk.multiply(csomk.sin()).multiply(csik.cos())),
  257.                                             xk.multiply(csomk.sin()).add(yk.multiply(csomk.cos()).multiply(csik.cos())),
  258.                                             yk.multiply(csik.sin()));
  259.         return new FieldPVCoordinates<>(positionWithDerivatives);

  261.     }

  263.     /**
  264.      * Gets the duration from GNSS Reference epoch.
  265.      * <p>This takes the GNSS week roll-over into account.</p>
  266.      * @param date the considered date
  267.      * @return the duration from GNSS orbit Reference epoch (s)
  268.      */
  269.     private T getTk(final FieldAbsoluteDate<T> date) {
  270.         // Time from ephemeris reference epoch
  271.         T tk = date.durationFrom(orbitalElements.getDate());
  272.         // Adjusts the time to take roll over week into account
  273.         while (tk.getReal() > 0.5 * orbitalElements.getCycleDuration()) {
  274.             tk = tk.subtract(orbitalElements.getCycleDuration());
  275.         }
  276.         while (tk.getReal() < -0.5 * orbitalElements.getCycleDuration()) {
  277.             tk = tk.add(orbitalElements.getCycleDuration());
  278.         }
  279.         // Returns the time from ephemeris reference epoch
  280.         return tk;
  281.     }

  283.     /** {@inheritDoc} */
  284.     @Override
  285.     public Frame getFrame() {
  286.         return eci;
  287.     }

  289.     /** {@inheritDoc} */
  290.     @Override
  291.     protected T getMass(final FieldAbsoluteDate<T> date) {
  292.         return getInitialState().getMass();
  293.     }

  295.     /** {@inheritDoc} */
  296.     @Override
  297.     public void resetInitialState(final FieldSpacecraftState<T> state) {
  298.         orbitalElements = buildOrbitalElements(state, orbitalElements, ecef, getAttitudeProvider(), state.getMass());
  299.         final FieldOrbit<T> orbit = propagateOrbit(orbitalElements.getDate(), defaultParameters());
  300.         final FieldAttitude<T> attitude = getAttitudeProvider().getAttitude(orbit, orbit.getDate(), orbit.getFrame());
  301.         super.resetInitialState(new FieldSpacecraftState<>(orbit, attitude).withMass(state.getMass()));
  302.     }

  304.     /** {@inheritDoc} */
  305.     @Override
  306.     protected void resetIntermediateState(final FieldSpacecraftState<T> state, final boolean forward) {
  307.         resetInitialState(state);
  308.     }

  310.     /**
  311.      * Build orbital elements from initial state.
  312.      * <p>
  313.      * This method is roughly the inverse of {@link #propagateInEcef(FieldAbsoluteDate, CalculusFieldElement[])},
  314.      * except it starts from a state in inertial frame
  315.      * </p>
  316.      *
  317.      * @param <T> type of the field elements
  318.      * @param initialState    initial state
  319.      * @param nonKeplerianElements non-Keplerian orbital elements (the Keplerian orbital elements will be overridden)
  320.      * @param ecef            Earth Centered Earth Fixed frame
  321.      * @param provider        attitude provider
  322.      * @param mass            satellite mass (kg)
  323.      * @return orbital elements that generate the {@code initialState} when used with a propagator
  324.      */
  325.     private static <T extends CalculusFieldElement<T>> FieldGnssOrbitalElements<T, ?>
  326.         buildOrbitalElements(final FieldSpacecraftState<T> initialState,
  327.                              final FieldGnssOrbitalElements<T, ?> nonKeplerianElements,
  328.                              final Frame ecef, final AttitudeProvider provider,
  329.                              final T mass) {

  331.         final Field<T> field = initialState.getDate().getField();

  333.         // get approximate initial orbit
  334.         final Frame frozenEcef = ecef.getFrozenFrame(initialState.getFrame(),
  335.                                                      initialState.getDate().toAbsoluteDate(),
  336.                                                      "frozen");
  337.         final FieldKeplerianOrbit<T> orbit = approximateInitialOrbit(initialState, nonKeplerianElements, frozenEcef);

  339.         // refine orbit using simple differential correction to reach target PV
  340.         final FieldPVCoordinates<T> targetPV = initialState.getPVCoordinates();
  341.         final FieldGnssOrbitalElements<FieldGradient<T>, ?> gElements = convert(nonKeplerianElements, orbit);
  342.         for (int i = 0; i < MAX_ITER; ++i) {

  344.             // get position-velocity derivatives with respect to initial orbit
  345.             final FieldGnssPropagator<FieldGradient<T>> gPropagator =
  346.                 new FieldGnssPropagator<>(gElements, initialState.getFrame(), ecef, provider,
  347.                                           gElements.getMu().newInstance(mass));
  348.             final FieldPVCoordinates<FieldGradient<T>> gPV = gPropagator.getInitialState().getPVCoordinates();

  350.             // compute Jacobian matrix
  351.             final FieldMatrix<T> jacobian = MatrixUtils.createFieldMatrix(field, FREE_PARAMETERS, FREE_PARAMETERS);
  352.             jacobian.setRow(0, gPV.getPosition().getX().getGradient());
  353.             jacobian.setRow(1, gPV.getPosition().getY().getGradient());
  354.             jacobian.setRow(2, gPV.getPosition().getZ().getGradient());
  355.             jacobian.setRow(3, gPV.getVelocity().getX().getGradient());
  356.             jacobian.setRow(4, gPV.getVelocity().getY().getGradient());
  357.             jacobian.setRow(5, gPV.getVelocity().getZ().getGradient());

  359.             // linear correction to get closer to target PV
  360.             final FieldVector<T> residuals = MatrixUtils.createFieldVector(field, FREE_PARAMETERS);
  361.             residuals.setEntry(0, targetPV.getPosition().getX().subtract(gPV.getPosition().getX().getValue()));
  362.             residuals.setEntry(1, targetPV.getPosition().getY().subtract(gPV.getPosition().getY().getValue()));
  363.             residuals.setEntry(2, targetPV.getPosition().getZ().subtract(gPV.getPosition().getZ().getValue()));
  364.             residuals.setEntry(3, targetPV.getVelocity().getX().subtract(gPV.getVelocity().getX().getValue()));
  365.             residuals.setEntry(4, targetPV.getVelocity().getY().subtract(gPV.getVelocity().getY().getValue()));
  366.             residuals.setEntry(5, targetPV.getVelocity().getZ().subtract(gPV.getVelocity().getZ().getValue()));
  367.             final FieldVector<T> correction = new FieldQRDecomposition<>(jacobian, field.getZero().newInstance(EPS)).
  368.                                               getSolver().
  369.                                               solve(residuals);

  371.             // update initial orbit
  372.             gElements.setSma(gElements.getSma().add(correction.getEntry(0)));
  373.             gElements.setE(gElements.getE().add(correction.getEntry(1)));
  374.             gElements.setI0(gElements.getI0().add(correction.getEntry(2)));
  375.             gElements.setPa(gElements.getPa().add(correction.getEntry(3)));
  376.             gElements.setOmega0(gElements.getOmega0().add(correction.getEntry(4)));
  377.             gElements.setM0(gElements.getM0().add(correction.getEntry(5)));

  379.             final double deltaP = FastMath.sqrt(residuals.getEntry(0).getReal() * residuals.getEntry(0).getReal() +
  380.                                                 residuals.getEntry(1).getReal() * residuals.getEntry(1).getReal() +
  381.                                                 residuals.getEntry(2).getReal() * residuals.getEntry(2).getReal());
  382.             final double deltaV = FastMath.sqrt(residuals.getEntry(3).getReal() * residuals.getEntry(3).getReal() +
  383.                                                 residuals.getEntry(4).getReal() * residuals.getEntry(4).getReal() +
  384.                                                 residuals.getEntry(5).getReal() * residuals.getEntry(5).getReal());

  386.             if (deltaP < TOL_P && deltaV < TOL_V) {
  387.                 break;
  388.             }

  390.         }

  392.         return gElements.changeField(FieldGradient::getValue);

  394.     }

  396.     /** Compute approximate initial orbit.
  397.      * @param <T> type of the field elements
  398.      * @param initialState         initial state
  399.      * @param nonKeplerianElements non-Keplerian orbital elements (the Keplerian orbital elements will be ignored)
  400.      * @param frozenEcef           inertial frame aligned with Earth Centered Earth Fixed frame at orbit date
  401.      * @return approximate initial orbit that generate a state close to {@code initialState}
  402.      */
  403.     private static <T extends CalculusFieldElement<T>> FieldKeplerianOrbit<T>
  404.         approximateInitialOrbit(final FieldSpacecraftState<T> initialState,
  405.                                 final FieldGnssOrbitalElements<T, ?> nonKeplerianElements,
  406.                                 final Frame frozenEcef) {

  408.         // rotate the state to a frame that is inertial but aligned with Earth frame,
  409.         // as analytical model is expressed in Earth frame
  410.         final FieldPVCoordinates<T> pv = initialState.getPVCoordinates(frozenEcef);
  411.         final FieldVector3D<T> p  = pv.getPosition();
  412.         final FieldVector3D<T>      v  = pv.getVelocity();

  414.         // compute Keplerian orbital parameters
  415.         final T   rk  = p.getNorm();

  417.         // compute orbital plane orientation
  418.         final FieldVector3D<T> normal = pv.getMomentum().normalize();
  419.         final T   cosIk  = normal.getZ();
  420.         final T   ik     = FieldVector3D.angle(normal, Vector3D.PLUS_K);

  422.         // compute position in orbital plane
  423.         final T   q   = FastMath.hypot(normal.getX(), normal.getY());
  424.         final T   cos = normal.getY().negate().divide(q);
  425.         final T   sin =  normal.getX().divide(q);
  426.         final T   xk  =  p.getX().multiply(cos).add(p.getY().multiply(sin));
  427.         final T   yk  = p.getY().multiply(cos).subtract(p.getX().multiply(sin)).divide(cosIk);

  429.         // corrected latitude argument
  430.         final T   uk  = FastMath.atan2(yk, xk);

  432.         // recover latitude argument before correction, using a fixed-point method
  433.         T phi = uk;
  434.         for (int i = 0; i < MAX_ITER; ++i) {
  435.             final T previous = phi;
  436.             final FieldSinCos<T> cs2Phi = FastMath.sinCos(phi.multiply(2));
  437.             phi = uk.subtract(cs2Phi.cos().multiply(nonKeplerianElements.getCuc()).add(cs2Phi.sin().multiply(nonKeplerianElements.getCus())));
  438.             if (FastMath.abs(phi.subtract(previous).getReal()) <= EPS) {
  439.                 break;
  440.             }
  441.         }
  442.         final FieldSinCos<T> cs2phi = FastMath.sinCos(phi.multiply(2));

  444.         // recover plane orientation before correction
  445.         // here, we know that tk = 0 since our orbital elements will be at initial state date
  446.         final T i0  = ik.subtract(cs2phi.cos().multiply(nonKeplerianElements.getCic()).add(cs2phi.sin().multiply(nonKeplerianElements.getCis())));
  447.         final T om0 = FastMath.atan2(sin, cos).
  448.                       add(nonKeplerianElements.getAngularVelocity() * nonKeplerianElements.getTime());

  450.         // recover eccentricity and anomaly
  451.         final T mu = initialState.getOrbit().getMu();
  452.         final T rV2OMu           = rk.multiply(v.getNormSq()).divide(mu);
  453.         final T sma              = rk.divide(rV2OMu.negate().add(2));
  454.         final T eCosE            = rV2OMu.subtract(1);
  455.         final T eSinE            = FieldVector3D.dotProduct(p, v).divide(FastMath.sqrt(mu.multiply(sma)));
  456.         final T e                = FastMath.hypot(eCosE, eSinE);
  457.         final T eccentricAnomaly = FastMath.atan2(eSinE, eCosE);
  458.         final T aop              = phi.subtract(eccentricAnomaly);
  459.         final T meanAnomaly      = FieldKeplerianAnomalyUtility.ellipticEccentricToMean(e, eccentricAnomaly);

  461.         return new FieldKeplerianOrbit<>(sma, e, i0, aop, om0, meanAnomaly, PositionAngleType.MEAN,
  462.                                          PositionAngleType.MEAN, frozenEcef,
  463.                                          initialState.getDate(), mu);

  465.     }

  467.     /** Convert orbital elements to gradient.
  468.      * @param <T> type of the field elements
  469.      * @param elements   primitive double elements
  470.      * @param orbit      Keplerian orbit
  471.      * @return converted elements, set up as gradient relative to Keplerian orbit
  472.      */
  473.     private static <T extends CalculusFieldElement<T>> FieldGnssOrbitalElements<FieldGradient<T>, ?>
  474.         convert(final FieldGnssOrbitalElements<T, ?> elements, final FieldKeplerianOrbit<T> orbit) {

  476.         final FieldGnssOrbitalElements<FieldGradient<T>, ?> gElements =
  477.             elements.changeField(t -> FieldGradient.constant(FREE_PARAMETERS, t));

  479.         // Keplerian parameters
  480.         gElements.setSma(FieldGradient.variable(FREE_PARAMETERS, 0, orbit.getA()));
  481.         gElements.setE(FieldGradient.variable(FREE_PARAMETERS, 1, orbit.getE()));
  482.         gElements.setI0(FieldGradient.variable(FREE_PARAMETERS, 2, orbit.getI()));
  483.         gElements.setPa(FieldGradient.variable(FREE_PARAMETERS, 3, orbit.getPerigeeArgument()));
  484.         gElements.setOmega0(FieldGradient.variable(FREE_PARAMETERS, 4, orbit.getRightAscensionOfAscendingNode()));
  485.         gElements.setM0(FieldGradient.variable(FREE_PARAMETERS, 5, orbit.getMeanAnomaly()));

  487.         return gElements;

  489.     }

  491. }
Created with JaCoCo 0.8.12.202403310830