FieldKeplerianOrbit.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.orbits;


  18. import java.util.List;
  19. import java.util.stream.Collectors;
  20. import java.util.stream.Stream;

  21. import org.hipparchus.CalculusFieldElement;
  22. import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative1;
  23. import org.hipparchus.analysis.interpolation.FieldHermiteInterpolator;
  24. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  25. import org.hipparchus.util.FastMath;
  26. import org.hipparchus.util.FieldSinCos;
  27. import org.hipparchus.util.MathArrays;
  28. import org.hipparchus.util.MathUtils;
  29. import org.orekit.errors.OrekitException;
  30. import org.orekit.errors.OrekitIllegalArgumentException;
  31. import org.orekit.errors.OrekitInternalError;
  32. import org.orekit.errors.OrekitMessages;
  33. import org.orekit.frames.Frame;
  34. import org.orekit.time.FieldAbsoluteDate;
  35. import org.orekit.utils.FieldPVCoordinates;
  36. import org.orekit.utils.TimeStampedFieldPVCoordinates;


  37. /**
  38.  * This class handles traditional Keplerian orbital parameters.

  39.  * <p>
  40.  * The parameters used internally are the classical Keplerian elements:
  41.  *   <pre>
  42.  *     a
  43.  *     e
  44.  *     i
  45.  *     ω
  46.  *     Ω
  47.  *     v
  48.  *   </pre>
  49.  * where ω stands for the Perigee Argument, Ω stands for the
  50.  * Right Ascension of the Ascending Node and v stands for the true anomaly.
  51.  * <p>
  52.  * This class supports hyperbolic orbits, using the convention that semi major
  53.  * axis is negative for such orbits (and of course eccentricity is greater than 1).
  54.  * </p>
  55.  * <p>
  56.  * When orbit is either equatorial or circular, some Keplerian elements
  57.  * (more precisely ω and Ω) become ambiguous so this class should not
  58.  * be used for such orbits. For this reason, {@link EquinoctialOrbit equinoctial
  59.  * orbits} is the recommended way to represent orbits.
  60.  * </p>

  61.  * <p>
  62.  * The instance <code>KeplerianOrbit</code> is guaranteed to be immutable.
  63.  * </p>
  64.  * @see     Orbit
  65.  * @see    CircularOrbit
  66.  * @see    CartesianOrbit
  67.  * @see    EquinoctialOrbit
  68.  * @author Luc Maisonobe
  69.  * @author Guylaine Prat
  70.  * @author Fabien Maussion
  71.  * @author V&eacute;ronique Pommier-Maurussane
  72.  * @author Andrea Antolino
  73.  * @since 9.0
  74.  */
  75. public class FieldKeplerianOrbit<T extends CalculusFieldElement<T>> extends FieldOrbit<T> {

  76.     /** Name of the eccentricity parameter. */
  77.     private static final String ECCENTRICITY = "eccentricity";

  78.     /** Semi-major axis (m). */
  79.     private final T a;

  80.     /** Eccentricity. */
  81.     private final T e;

  82.     /** Inclination (rad). */
  83.     private final T i;

  84.     /** Perigee Argument (rad). */
  85.     private final T pa;

  86.     /** Right Ascension of Ascending Node (rad). */
  87.     private final T raan;

  88.     /** True anomaly (rad). */
  89.     private final T v;

  90.     /** Semi-major axis derivative (m/s). */
  91.     private final T aDot;

  92.     /** Eccentricity derivative. */
  93.     private final T eDot;

  94.     /** Inclination derivative (rad/s). */
  95.     private final T iDot;

  96.     /** Perigee Argument derivative (rad/s). */
  97.     private final T paDot;

  98.     /** Right Ascension of Ascending Node derivative (rad/s). */
  99.     private final T raanDot;

  100.     /** True anomaly derivative (rad/s). */
  101.     private final T vDot;

  102.     /** Partial Cartesian coordinates (position and velocity are valid, acceleration may be missing). */
  103.     private FieldPVCoordinates<T> partialPV;

  104.     /** Identity element. */
  105.     private final T one;

  106.     /** Zero element. */
  107.     private final T zero;

  108.     /** Third Canonical Vector. */
  109.     private final FieldVector3D<T> PLUS_K;

  110.     /** Creates a new instance.
  111.      * @param a  semi-major axis (m), negative for hyperbolic orbits
  112.      * @param e eccentricity (positive or equal to 0)
  113.      * @param i inclination (rad)
  114.      * @param pa perigee argument (ω, rad)
  115.      * @param raan right ascension of ascending node (Ω, rad)
  116.      * @param anomaly mean, eccentric or true anomaly (rad)
  117.      * @param type type of anomaly
  118.      * @param frame the frame in which the parameters are defined
  119.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  120.      * @param date date of the orbital parameters
  121.      * @param mu central attraction coefficient (m³/s²)
  122.      * @exception IllegalArgumentException if frame is not a {@link
  123.      * Frame#isPseudoInertial pseudo-inertial frame} or a and e don't match for hyperbolic orbits,
  124.      * or v is out of range for hyperbolic orbits
  125.      */
  126.     public FieldKeplerianOrbit(final T a, final T e, final T i,
  127.                                final T pa, final T raan,
  128.                                final T anomaly, final PositionAngle type,
  129.                                final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
  130.         throws IllegalArgumentException {
  131.         this(a, e, i, pa, raan, anomaly,
  132.              null, null, null, null, null, null,
  133.              type, frame, date, mu);
  134.     }

  135.     /** Creates a new instance.
  136.      * @param a  semi-major axis (m), negative for hyperbolic orbits
  137.      * @param e eccentricity (positive or equal to 0)
  138.      * @param i inclination (rad)
  139.      * @param pa perigee argument (ω, rad)
  140.      * @param raan right ascension of ascending node (Ω, rad)
  141.      * @param anomaly mean, eccentric or true anomaly (rad)
  142.      * @param aDot  semi-major axis derivative, null if unknown (m/s)
  143.      * @param eDot eccentricity derivative, null if unknown
  144.      * @param iDot inclination derivative, null if unknown (rad/s)
  145.      * @param paDot perigee argument derivative, null if unknown (rad/s)
  146.      * @param raanDot right ascension of ascending node derivative, null if unknown (rad/s)
  147.      * @param anomalyDot mean, eccentric or true anomaly derivative, null if unknown (rad/s)
  148.      * @param type type of anomaly
  149.      * @param frame the frame in which the parameters are defined
  150.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  151.      * @param date date of the orbital parameters
  152.      * @param mu central attraction coefficient (m³/s²)
  153.      * @exception IllegalArgumentException if frame is not a {@link
  154.      * Frame#isPseudoInertial pseudo-inertial frame} or a and e don't match for hyperbolic orbits,
  155.      * or v is out of range for hyperbolic orbits
  156.      */
  157.     public FieldKeplerianOrbit(final T a, final T e, final T i,
  158.                                final T pa, final T raan, final T anomaly,
  159.                                final T aDot, final T eDot, final T iDot,
  160.                                final T paDot, final T raanDot, final T anomalyDot,
  161.                                final PositionAngle type,
  162.                                final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
  163.         throws IllegalArgumentException {
  164.         super(frame, date, mu);
  165.         if (a.multiply(e.negate().add(1)).getReal() < 0) {
  166.             throw new OrekitIllegalArgumentException(OrekitMessages.ORBIT_A_E_MISMATCH_WITH_CONIC_TYPE, a.getReal(), e.getReal());
  167.         }

  168.         // Checking eccentricity range
  169.         checkParameterRangeInclusive(ECCENTRICITY, e.getReal(), 0.0, Double.POSITIVE_INFINITY);

  170.         this.a       =    a;
  171.         this.aDot    =    aDot;
  172.         this.e       =    e;
  173.         this.eDot    =    eDot;
  174.         this.i       =    i;
  175.         this.iDot    =    iDot;
  176.         this.pa      =    pa;
  177.         this.paDot   =    paDot;
  178.         this.raan    =    raan;
  179.         this.raanDot =    raanDot;

  180.         /** Identity element. */
  181.         this.one = a.getField().getOne();

  182.         /** Zero element. */
  183.         this.zero = a.getField().getZero();

  184.         /**Third canonical vector. */
  185.         this.PLUS_K = FieldVector3D.getPlusK(a.getField());

  186.         if (hasDerivatives()) {
  187.             final FieldUnivariateDerivative1<T> eUD        = new FieldUnivariateDerivative1<>(e, eDot);
  188.             final FieldUnivariateDerivative1<T> anomalyUD  = new FieldUnivariateDerivative1<>(anomaly,  anomalyDot);
  189.             final FieldUnivariateDerivative1<T> vUD;
  190.             switch (type) {
  191.                 case MEAN :
  192.                     vUD = (a.getReal() < 0) ?
  193.                           FieldKeplerianAnomalyUtility.hyperbolicMeanToTrue(eUD, anomalyUD) :
  194.                           FieldKeplerianAnomalyUtility.ellipticMeanToTrue(eUD, anomalyUD);
  195.                     break;
  196.                 case ECCENTRIC :
  197.                     vUD = (a.getReal() < 0) ?
  198.                           FieldKeplerianAnomalyUtility.hyperbolicEccentricToTrue(eUD, anomalyUD) :
  199.                           FieldKeplerianAnomalyUtility.ellipticEccentricToTrue(eUD, anomalyUD);
  200.                     break;
  201.                 case TRUE :
  202.                     vUD = anomalyUD;
  203.                     break;
  204.                 default : // this should never happen
  205.                     throw new OrekitInternalError(null);
  206.             }
  207.             this.v    = vUD.getValue();
  208.             this.vDot = vUD.getDerivative(1);
  209.         } else {
  210.             switch (type) {
  211.                 case MEAN :

  212.                     this.v = (a.getReal() < 0) ?
  213.                              FieldKeplerianAnomalyUtility.hyperbolicMeanToTrue(e, anomaly) :
  214.                              FieldKeplerianAnomalyUtility.ellipticMeanToTrue(e, anomaly);

  215.                     break;
  216.                 case ECCENTRIC :
  217.                     this.v = (a.getReal() < 0) ?
  218.                              FieldKeplerianAnomalyUtility.hyperbolicEccentricToTrue(e, anomaly) :
  219.                              FieldKeplerianAnomalyUtility.ellipticEccentricToTrue(e, anomaly);

  220.                     break;
  221.                 case TRUE :
  222.                     this.v = anomaly;
  223.                     break;
  224.                 default : // this should never happen
  225.                     throw new OrekitInternalError(null);
  226.             }
  227.             this.vDot = null;
  228.         }

  229.         // check true anomaly range
  230.         if (e.multiply(v.cos()).add(1).getReal() <= 0) {
  231.             final double vMax = e.reciprocal().negate().acos().getReal();
  232.             throw new OrekitIllegalArgumentException(OrekitMessages.ORBIT_ANOMALY_OUT_OF_HYPERBOLIC_RANGE,
  233.                                                      v.getReal(), e.getReal(), -vMax, vMax);
  234.         }

  235.         this.partialPV = null;

  236.     }

  237.     /** Constructor from Cartesian parameters.
  238.      *
  239.      * <p> The acceleration provided in {@code FieldPVCoordinates} is accessible using
  240.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  241.      * use {@code mu} and the position to compute the acceleration, including
  242.      * {@link #shiftedBy(CalculusFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
  243.      *
  244.      * @param pvCoordinates the PVCoordinates of the satellite
  245.      * @param frame the frame in which are defined the {@link FieldPVCoordinates}
  246.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  247.      * @param mu central attraction coefficient (m³/s²)
  248.      * @exception IllegalArgumentException if frame is not a {@link
  249.      * Frame#isPseudoInertial pseudo-inertial frame}
  250.      */
  251.     public FieldKeplerianOrbit(final TimeStampedFieldPVCoordinates<T> pvCoordinates,
  252.                                final Frame frame, final T mu)
  253.         throws IllegalArgumentException {
  254.         this(pvCoordinates, frame, mu, hasNonKeplerianAcceleration(pvCoordinates, mu));
  255.     }

  256.     /** Constructor from Cartesian parameters.
  257.      *
  258.      * <p> The acceleration provided in {@code FieldPVCoordinates} is accessible using
  259.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  260.      * use {@code mu} and the position to compute the acceleration, including
  261.      * {@link #shiftedBy(CalculusFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
  262.      *
  263.      * @param pvCoordinates the PVCoordinates of the satellite
  264.      * @param frame the frame in which are defined the {@link FieldPVCoordinates}
  265.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  266.      * @param mu central attraction coefficient (m³/s²)
  267.      * @param reliableAcceleration if true, the acceleration is considered to be reliable
  268.      * @exception IllegalArgumentException if frame is not a {@link
  269.      * Frame#isPseudoInertial pseudo-inertial frame}
  270.      */
  271.     private FieldKeplerianOrbit(final TimeStampedFieldPVCoordinates<T> pvCoordinates,
  272.                                 final Frame frame, final T mu,
  273.                                 final boolean reliableAcceleration)
  274.         throws IllegalArgumentException {

  275.         super(pvCoordinates, frame, mu);

  276.         // identity element
  277.         this.one = pvCoordinates.getPosition().getX().getField().getOne();

  278.         // zero element
  279.         this.zero = one.getField().getZero();

  280.         // third canonical vector
  281.         this.PLUS_K = FieldVector3D.getPlusK(one.getField());

  282.         // compute inclination
  283.         final FieldVector3D<T> momentum = pvCoordinates.getMomentum();
  284.         final T m2 = momentum.getNormSq();

  285.         i = FieldVector3D.angle(momentum, PLUS_K);
  286.         // compute right ascension of ascending node
  287.         raan = FieldVector3D.crossProduct(PLUS_K, momentum).getAlpha();
  288.         // preliminary computations for parameters depending on orbit shape (elliptic or hyperbolic)
  289.         final FieldVector3D<T> pvP     = pvCoordinates.getPosition();
  290.         final FieldVector3D<T> pvV     = pvCoordinates.getVelocity();
  291.         final FieldVector3D<T> pvA     = pvCoordinates.getAcceleration();

  292.         final T   r2      = pvP.getNormSq();
  293.         final T   r       = r2.sqrt();
  294.         final T   V2      = pvV.getNormSq();
  295.         final T   rV2OnMu = r.multiply(V2).divide(mu);

  296.         // compute semi-major axis (will be negative for hyperbolic orbits)
  297.         a = r.divide(rV2OnMu.negate().add(2.0));
  298.         final T muA = a.multiply(mu);

  299.         // compute true anomaly
  300.         if (a.getReal() > 0) {
  301.             // elliptic or circular orbit
  302.             final T eSE = FieldVector3D.dotProduct(pvP, pvV).divide(muA.sqrt());
  303.             final T eCE = rV2OnMu.subtract(1);
  304.             e = (eSE.multiply(eSE).add(eCE.multiply(eCE))).sqrt();
  305.             v = FieldKeplerianAnomalyUtility.ellipticEccentricToTrue(e, eSE.atan2(eCE));
  306.         } else {
  307.             // hyperbolic orbit
  308.             final T eSH = FieldVector3D.dotProduct(pvP, pvV).divide(muA.negate().sqrt());
  309.             final T eCH = rV2OnMu.subtract(1);
  310.             e = (m2.negate().divide(muA).add(1)).sqrt();
  311.             v = FieldKeplerianAnomalyUtility.hyperbolicEccentricToTrue(e, (eCH.add(eSH)).divide(eCH.subtract(eSH)).log().divide(2));
  312.         }

  313.         // Checking eccentricity range
  314.         checkParameterRangeInclusive(ECCENTRICITY, e.getReal(), 0.0, Double.POSITIVE_INFINITY);

  315.         // compute perigee argument
  316.         final FieldVector3D<T> node = new FieldVector3D<>(raan, zero);
  317.         final T px = FieldVector3D.dotProduct(pvP, node);
  318.         final T py = FieldVector3D.dotProduct(pvP, FieldVector3D.crossProduct(momentum, node)).divide(m2.sqrt());
  319.         pa = py.atan2(px).subtract(v);

  320.         partialPV = pvCoordinates;

  321.         if (reliableAcceleration) {
  322.             // we have a relevant acceleration, we can compute derivatives

  323.             final T[][] jacobian = MathArrays.buildArray(a.getField(), 6, 6);
  324.             getJacobianWrtCartesian(PositionAngle.MEAN, jacobian);

  325.             final FieldVector3D<T> keplerianAcceleration    = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(mu.negate()), pvP);
  326.             final FieldVector3D<T> nonKeplerianAcceleration = pvA.subtract(keplerianAcceleration);
  327.             final T   aX                       = nonKeplerianAcceleration.getX();
  328.             final T   aY                       = nonKeplerianAcceleration.getY();
  329.             final T   aZ                       = nonKeplerianAcceleration.getZ();
  330.             aDot    = jacobian[0][3].multiply(aX).add(jacobian[0][4].multiply(aY)).add(jacobian[0][5].multiply(aZ));
  331.             eDot    = jacobian[1][3].multiply(aX).add(jacobian[1][4].multiply(aY)).add(jacobian[1][5].multiply(aZ));
  332.             iDot    = jacobian[2][3].multiply(aX).add(jacobian[2][4].multiply(aY)).add(jacobian[2][5].multiply(aZ));
  333.             paDot   = jacobian[3][3].multiply(aX).add(jacobian[3][4].multiply(aY)).add(jacobian[3][5].multiply(aZ));
  334.             raanDot = jacobian[4][3].multiply(aX).add(jacobian[4][4].multiply(aY)).add(jacobian[4][5].multiply(aZ));

  335.             // in order to compute true anomaly derivative, we must compute
  336.             // mean anomaly derivative including Keplerian motion and convert to true anomaly
  337.             final T MDot = getKeplerianMeanMotion().
  338.                            add(jacobian[5][3].multiply(aX)).add(jacobian[5][4].multiply(aY)).add(jacobian[5][5].multiply(aZ));
  339.             final FieldUnivariateDerivative1<T> eUD = new FieldUnivariateDerivative1<>(e, eDot);
  340.             final FieldUnivariateDerivative1<T> MUD = new FieldUnivariateDerivative1<>(getMeanAnomaly(), MDot);
  341.             final FieldUnivariateDerivative1<T> vUD = (a.getReal() < 0) ?
  342.                                             FieldKeplerianAnomalyUtility.hyperbolicMeanToTrue(eUD, MUD) :
  343.                                             FieldKeplerianAnomalyUtility.ellipticMeanToTrue(eUD, MUD);
  344.             vDot = vUD.getDerivative(1);

  345.         } else {
  346.             // acceleration is either almost zero or NaN,
  347.             // we assume acceleration was not known
  348.             // we don't set up derivatives
  349.             aDot    = null;
  350.             eDot    = null;
  351.             iDot    = null;
  352.             paDot   = null;
  353.             raanDot = null;
  354.             vDot    = null;
  355.         }

  356.     }

  357.     /** Constructor from Cartesian parameters.
  358.      *
  359.      * <p> The acceleration provided in {@code FieldPVCoordinates} is accessible using
  360.      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
  361.      * use {@code mu} and the position to compute the acceleration, including
  362.      * {@link #shiftedBy(CalculusFieldElement)} and {@link #getPVCoordinates(FieldAbsoluteDate, Frame)}.
  363.      *
  364.      * @param FieldPVCoordinates the PVCoordinates of the satellite
  365.      * @param frame the frame in which are defined the {@link FieldPVCoordinates}
  366.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  367.      * @param date date of the orbital parameters
  368.      * @param mu central attraction coefficient (m³/s²)
  369.      * @exception IllegalArgumentException if frame is not a {@link
  370.      * Frame#isPseudoInertial pseudo-inertial frame}
  371.      */
  372.     public FieldKeplerianOrbit(final FieldPVCoordinates<T> FieldPVCoordinates,
  373.                                final Frame frame, final FieldAbsoluteDate<T> date, final T mu)
  374.         throws IllegalArgumentException {
  375.         this(new TimeStampedFieldPVCoordinates<>(date, FieldPVCoordinates), frame, mu);
  376.     }

  377.     /** Constructor from any kind of orbital parameters.
  378.      * @param op orbital parameters to copy
  379.      */
  380.     public FieldKeplerianOrbit(final FieldOrbit<T> op) {
  381.         this(op.getPVCoordinates(), op.getFrame(), op.getMu(), op.hasDerivatives());
  382.     }

  383.     /** {@inheritDoc} */
  384.     public OrbitType getType() {
  385.         return OrbitType.KEPLERIAN;
  386.     }

  387.     /** {@inheritDoc} */
  388.     public T getA() {
  389.         return a;
  390.     }

  391.     /** {@inheritDoc} */
  392.     public T getADot() {
  393.         return aDot;
  394.     }

  395.     /** {@inheritDoc} */
  396.     public T getE() {
  397.         return e;
  398.     }

  399.     /** {@inheritDoc} */
  400.     public T getEDot() {
  401.         return eDot;
  402.     }

  403.     /** {@inheritDoc} */
  404.     public T getI() {
  405.         return i;
  406.     }

  407.     /** {@inheritDoc} */
  408.     public T getIDot() {
  409.         return iDot;
  410.     }

  411.     /** Get the perigee argument.
  412.      * @return perigee argument (rad)
  413.      */
  414.     public T getPerigeeArgument() {
  415.         return pa;
  416.     }

  417.     /** Get the perigee argument derivative.
  418.      * <p>
  419.      * If the orbit was created without derivatives, the value returned is null.
  420.      * </p>
  421.      * @return perigee argument derivative (rad/s)
  422.      */
  423.     public T getPerigeeArgumentDot() {
  424.         return paDot;
  425.     }

  426.     /** Get the right ascension of the ascending node.
  427.      * @return right ascension of the ascending node (rad)
  428.      */
  429.     public T getRightAscensionOfAscendingNode() {
  430.         return raan;
  431.     }

  432.     /** Get the right ascension of the ascending node derivative.
  433.      * <p>
  434.      * If the orbit was created without derivatives, the value returned is null.
  435.      * </p>
  436.      * @return right ascension of the ascending node derivative (rad/s)
  437.      */
  438.     public T getRightAscensionOfAscendingNodeDot() {
  439.         return raanDot;
  440.     }

  441.     /** Get the true anomaly.
  442.      * @return true anomaly (rad)
  443.      */
  444.     public T getTrueAnomaly() {
  445.         return v;
  446.     }

  447.     /** Get the true anomaly derivative.
  448.      * <p>
  449.      * If the orbit was created without derivatives, the value returned is null.
  450.      * </p>
  451.      * @return true anomaly derivative (rad/s)
  452.      */
  453.     public T getTrueAnomalyDot() {
  454.         return vDot;
  455.     }

  456.     /** Get the eccentric anomaly.
  457.      * @return eccentric anomaly (rad)
  458.      */
  459.     public T getEccentricAnomaly() {
  460.         return (a.getReal() < 0) ?
  461.                FieldKeplerianAnomalyUtility.hyperbolicTrueToEccentric(e, v) :
  462.                FieldKeplerianAnomalyUtility.ellipticTrueToEccentric(e, v);
  463.     }

  464.     /** Get the eccentric anomaly derivative.
  465.      * <p>
  466.      * If the orbit was created without derivatives, the value returned is null.
  467.      * </p>
  468.      * @return eccentric anomaly derivative (rad/s)
  469.      */
  470.     public T getEccentricAnomalyDot() {

  471.         if (!hasDerivatives()) {
  472.             return null;
  473.         }

  474.         final FieldUnivariateDerivative1<T> eUD = new FieldUnivariateDerivative1<>(e, eDot);
  475.         final FieldUnivariateDerivative1<T> vUD = new FieldUnivariateDerivative1<>(v, vDot);
  476.         final FieldUnivariateDerivative1<T> EUD = (a.getReal() < 0) ?
  477.                                                 FieldKeplerianAnomalyUtility.hyperbolicTrueToEccentric(eUD, vUD) :
  478.                                                 FieldKeplerianAnomalyUtility.ellipticTrueToEccentric(eUD, vUD);
  479.         return EUD.getDerivative(1);

  480.     }

  481.     /** Get the mean anomaly.
  482.      * @return mean anomaly (rad)
  483.      */
  484.     public T getMeanAnomaly() {
  485.         return (a.getReal() < 0) ?
  486.                FieldKeplerianAnomalyUtility.hyperbolicTrueToMean(e, v) :
  487.                FieldKeplerianAnomalyUtility.ellipticTrueToMean(e, v);
  488.     }

  489.     /** Get the mean anomaly derivative.
  490.      * <p>
  491.      * If the orbit was created without derivatives, the value returned is null.
  492.      * </p>
  493.      * @return mean anomaly derivative (rad/s)
  494.      */
  495.     public T getMeanAnomalyDot() {

  496.         if (!hasDerivatives()) {
  497.             return null;
  498.         }

  499.         final FieldUnivariateDerivative1<T> eUD = new FieldUnivariateDerivative1<>(e, eDot);
  500.         final FieldUnivariateDerivative1<T> vUD = new FieldUnivariateDerivative1<>(v, vDot);
  501.         final FieldUnivariateDerivative1<T> MUD = (a.getReal() < 0) ?
  502.                                                 FieldKeplerianAnomalyUtility.hyperbolicTrueToMean(eUD, vUD) :
  503.                                                 FieldKeplerianAnomalyUtility.ellipticTrueToMean(eUD, vUD);
  504.         return MUD.getDerivative(1);

  505.     }

  506.     /** Get the anomaly.
  507.      * @param type type of the angle
  508.      * @return anomaly (rad)
  509.      */
  510.     public T getAnomaly(final PositionAngle type) {
  511.         return (type == PositionAngle.MEAN) ? getMeanAnomaly() :
  512.                                               ((type == PositionAngle.ECCENTRIC) ? getEccentricAnomaly() :
  513.                                                                                    getTrueAnomaly());
  514.     }

  515.     /** Get the anomaly derivative.
  516.      * <p>
  517.      * If the orbit was created without derivatives, the value returned is null.
  518.      * </p>
  519.      * @param type type of the angle
  520.      * @return anomaly derivative (rad/s)
  521.      */
  522.     public T getAnomalyDot(final PositionAngle type) {
  523.         return (type == PositionAngle.MEAN) ? getMeanAnomalyDot() :
  524.                                               ((type == PositionAngle.ECCENTRIC) ? getEccentricAnomalyDot() :
  525.                                                                                    getTrueAnomalyDot());
  526.     }

  527.     /** {@inheritDoc} */
  528.     @Override
  529.     public boolean hasDerivatives() {
  530.         return aDot != null;
  531.     }

  532.     /** Computes the true anomaly from the elliptic eccentric anomaly.
  533.      * @param E eccentric anomaly (rad)
  534.      * @param e eccentricity
  535.      * @param <T> type of the field elements
  536.      * @return v the true anomaly
  537.      * @deprecated As of 11.3, replaced by
  538.      * {@link FieldKeplerianAnomalyUtility#ellipticEccentricToMean(CalculusFieldElement, CalculusFieldElement)}.
  539.      */
  540.     public static <T extends CalculusFieldElement<T>> T ellipticEccentricToTrue(final T E, final T e) {
  541.         return FieldKeplerianAnomalyUtility.ellipticEccentricToMean(e, E);
  542.     }

  543.     /** Computes the elliptic eccentric anomaly from the true anomaly.
  544.      * @param v true anomaly (rad)
  545.      * @param e eccentricity
  546.      * @param <T> type of the field elements
  547.      * @return E the elliptic eccentric anomaly
  548.      * @deprecated As of 11.3, replaced by
  549.      * {@link FieldKeplerianAnomalyUtility#ellipticTrueToEccentric(CalculusFieldElement, CalculusFieldElement)}.
  550.      */
  551.     public static <T extends CalculusFieldElement<T>> T trueToEllipticEccentric(final T v, final T e) {
  552.         return FieldKeplerianAnomalyUtility.ellipticTrueToEccentric(e, v);
  553.     }

  554.     /** Computes the true anomaly from the hyperbolic eccentric anomaly.
  555.      * @param H hyperbolic eccentric anomaly (rad)
  556.      * @param e eccentricity
  557.      * @param <T> type of the field elements
  558.      * @return v the true anomaly
  559.      * @deprecated As of 11.3, replaced by
  560.      * {@link FieldKeplerianAnomalyUtility#hyperbolicEccentricToTrue(CalculusFieldElement, CalculusFieldElement)}.
  561.      */
  562.     public static <T extends CalculusFieldElement<T>> T hyperbolicEccentricToTrue(final T H, final T e) {
  563.         return FieldKeplerianAnomalyUtility.hyperbolicEccentricToTrue(e, H);
  564.     }

  565.     /** Computes the hyperbolic eccentric anomaly from the true anomaly.
  566.      * @param v true anomaly (rad)
  567.      * @param e eccentricity
  568.      * @param <T> type of the field elements
  569.      * @return H the hyperbolic eccentric anomaly
  570.      * @deprecated As of 11.3, replaced by
  571.      * {@link FieldKeplerianAnomalyUtility#hyperbolicTrueToEccentric(CalculusFieldElement, CalculusFieldElement)}.
  572.      */
  573.     public static <T extends CalculusFieldElement<T>> T trueToHyperbolicEccentric(final T v, final T e) {
  574.         return FieldKeplerianAnomalyUtility.hyperbolicTrueToEccentric(e, v);
  575.     }

  576.     /** Computes the mean anomaly from the hyperbolic eccentric anomaly.
  577.      * @param H hyperbolic eccentric anomaly (rad)
  578.      * @param e eccentricity
  579.      * @param <T> type of the field elements
  580.      * @return M the mean anomaly
  581.      * @deprecated As of 11.3, replaced by
  582.      * {@link FieldKeplerianAnomalyUtility#hyperbolicEccentricToMean(CalculusFieldElement, CalculusFieldElement)}.
  583.      */
  584.     public static <T extends CalculusFieldElement<T>> T hyperbolicEccentricToMean(final T H, final T e) {
  585.         return FieldKeplerianAnomalyUtility.hyperbolicEccentricToMean(e, H);
  586.     }

  587.     /** Computes the elliptic eccentric anomaly from the mean anomaly.
  588.      * @param M mean anomaly (rad)
  589.      * @param e eccentricity
  590.      * @param <T> type of the field elements
  591.      * @return E the eccentric anomaly
  592.      * @deprecated As of 11.3, replaced by
  593.      * {@link FieldKeplerianAnomalyUtility#ellipticMeanToEccentric(CalculusFieldElement, CalculusFieldElement)}.
  594.      */
  595.     public static <T extends CalculusFieldElement<T>> T meanToEllipticEccentric(final T M, final T e) {
  596.         return FieldKeplerianAnomalyUtility.ellipticMeanToEccentric(e, M);
  597.     }

  598.     /** Computes the hyperbolic eccentric anomaly from the mean anomaly.
  599.      * @param M mean anomaly (rad)
  600.      * @param e eccentricity
  601.      * @param <T> Type of the field elements
  602.      * @return H the hyperbolic eccentric anomaly
  603.      * @deprecated As of 11.3, replaced by
  604.      * {@link FieldKeplerianAnomalyUtility#hyperbolicMeanToEccentric(CalculusFieldElement, CalculusFieldElement)}.
  605.      */
  606.     public static <T extends CalculusFieldElement<T>> T meanToHyperbolicEccentric(final T M, final T e) {
  607.         return FieldKeplerianAnomalyUtility.hyperbolicMeanToEccentric(e, M);
  608.     }

  609.     /** Computes the mean anomaly from the elliptic eccentric anomaly.
  610.      * @param E eccentric anomaly (rad)
  611.      * @param e eccentricity
  612.      * @param <T> type of the field elements
  613.      * @return M the mean anomaly
  614.      * @deprecated As of 11.3, replaced by
  615.      * {@link FieldKeplerianAnomalyUtility#ellipticEccentricToMean(CalculusFieldElement, CalculusFieldElement)}.
  616.      */
  617.     public static <T extends CalculusFieldElement<T>> T ellipticEccentricToMean(final T E, final T e) {
  618.         return FieldKeplerianAnomalyUtility.ellipticEccentricToMean(e, E);
  619.     }

  620.     /** {@inheritDoc} */
  621.     public T getEquinoctialEx() {
  622.         return e.multiply(pa.add(raan).cos());
  623.     }

  624.     /** {@inheritDoc} */
  625.     public T getEquinoctialExDot() {

  626.         if (!hasDerivatives()) {
  627.             return null;
  628.         }

  629.         final FieldUnivariateDerivative1<T> eUD    = new FieldUnivariateDerivative1<>(e,    eDot);
  630.         final FieldUnivariateDerivative1<T> paUD   = new FieldUnivariateDerivative1<>(pa,   paDot);
  631.         final FieldUnivariateDerivative1<T> raanUD = new FieldUnivariateDerivative1<>(raan, raanDot);
  632.         return eUD.multiply(paUD.add(raanUD).cos()).getDerivative(1);

  633.     }

  634.     /** {@inheritDoc} */
  635.     public T getEquinoctialEy() {
  636.         return  e.multiply((pa.add(raan)).sin());
  637.     }

  638.     /** {@inheritDoc} */
  639.     public T getEquinoctialEyDot() {

  640.         if (!hasDerivatives()) {
  641.             return null;
  642.         }

  643.         final FieldUnivariateDerivative1<T> eUD    = new FieldUnivariateDerivative1<>(e,    eDot);
  644.         final FieldUnivariateDerivative1<T> paUD   = new FieldUnivariateDerivative1<>(pa,   paDot);
  645.         final FieldUnivariateDerivative1<T> raanUD = new FieldUnivariateDerivative1<>(raan, raanDot);
  646.         return eUD.multiply(paUD.add(raanUD).sin()).getDerivative(1);

  647.     }

  648.     /** {@inheritDoc} */
  649.     public T getHx() {
  650.         // Check for equatorial retrograde orbit
  651.         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
  652.             return this.zero.add(Double.NaN);
  653.         }
  654.         return  raan.cos().multiply(i.divide(2).tan());
  655.     }

  656.     /** {@inheritDoc} */
  657.     public T getHxDot() {

  658.         if (!hasDerivatives()) {
  659.             return null;
  660.         }

  661.         // Check for equatorial retrograde orbit
  662.         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
  663.             return this.zero.add(Double.NaN);
  664.         }

  665.         final FieldUnivariateDerivative1<T> iUD    = new FieldUnivariateDerivative1<>(i,    iDot);
  666.         final FieldUnivariateDerivative1<T> raanUD = new FieldUnivariateDerivative1<>(raan, raanDot);
  667.         return raanUD.cos().multiply(iUD.multiply(0.5).tan()).getDerivative(1);

  668.     }

  669.     /** {@inheritDoc} */
  670.     public T getHy() {
  671.         // Check for equatorial retrograde orbit
  672.         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
  673.             return this.zero.add(Double.NaN);
  674.         }
  675.         return  raan.sin().multiply(i.divide(2).tan());
  676.     }

  677.     /** {@inheritDoc} */
  678.     public T getHyDot() {

  679.         if (!hasDerivatives()) {
  680.             return null;
  681.         }

  682.         // Check for equatorial retrograde orbit
  683.         if (FastMath.abs(i.subtract(i.getPi()).getReal()) < 1.0e-10) {
  684.             return this.zero.add(Double.NaN);
  685.         }

  686.         final FieldUnivariateDerivative1<T> iUD    = new FieldUnivariateDerivative1<>(i,    iDot);
  687.         final FieldUnivariateDerivative1<T> raanUD = new FieldUnivariateDerivative1<>(raan, raanDot);
  688.         return raanUD.sin().multiply(iUD.multiply(0.5).tan()).getDerivative(1);

  689.     }

  690.     /** {@inheritDoc} */
  691.     public T getLv() {
  692.         return pa.add(raan).add(v);
  693.     }

  694.     /** {@inheritDoc} */
  695.     public T getLvDot() {
  696.         return hasDerivatives() ?
  697.                paDot.add(raanDot).add(vDot) :
  698.                null;
  699.     }

  700.     /** {@inheritDoc} */
  701.     public T getLE() {
  702.         return pa.add(raan).add(getEccentricAnomaly());
  703.     }

  704.     /** {@inheritDoc} */
  705.     public T getLEDot() {
  706.         return hasDerivatives() ?
  707.                paDot.add(raanDot).add(getEccentricAnomalyDot()) :
  708.                null;
  709.     }

  710.     /** {@inheritDoc} */
  711.     public T getLM() {
  712.         return pa.add(raan).add(getMeanAnomaly());
  713.     }

  714.     /** {@inheritDoc} */
  715.     public T getLMDot() {
  716.         return hasDerivatives() ?
  717.                paDot.add(raanDot).add(getMeanAnomalyDot()) :
  718.                null;
  719.     }

  720.     /** Compute position and velocity but not acceleration.
  721.      */
  722.     private void computePVWithoutA() {

  723.         if (partialPV != null) {
  724.             // already computed
  725.             return;
  726.         }

  727.         // preliminary variables
  728.         final FieldSinCos<T> scRaan = FastMath.sinCos(raan);
  729.         final FieldSinCos<T> scPa   = FastMath.sinCos(pa);
  730.         final FieldSinCos<T> scI    = FastMath.sinCos(i);
  731.         final T cosRaan = scRaan.cos();
  732.         final T sinRaan = scRaan.sin();
  733.         final T cosPa   = scPa.cos();
  734.         final T sinPa   = scPa.sin();
  735.         final T cosI    = scI.cos();
  736.         final T sinI    = scI.sin();
  737.         final T crcp    = cosRaan.multiply(cosPa);
  738.         final T crsp    = cosRaan.multiply(sinPa);
  739.         final T srcp    = sinRaan.multiply(cosPa);
  740.         final T srsp    = sinRaan.multiply(sinPa);

  741.         // reference axes defining the orbital plane
  742.         final FieldVector3D<T> p = new FieldVector3D<>(crcp.subtract(cosI.multiply(srsp)),  srcp.add(cosI.multiply(crsp)), sinI.multiply(sinPa));
  743.         final FieldVector3D<T> q = new FieldVector3D<>(crsp.add(cosI.multiply(srcp)).negate(), cosI.multiply(crcp).subtract(srsp), sinI.multiply(cosPa));

  744.         if (a.getReal() > 0) {

  745.             // elliptical case

  746.             // elliptic eccentric anomaly
  747.             final T uME2             = e.negate().add(1).multiply(e.add(1));
  748.             final T s1Me2            = uME2.sqrt();
  749.             final FieldSinCos<T> scE = FastMath.sinCos(getEccentricAnomaly());
  750.             final T cosE             = scE.cos();
  751.             final T sinE             = scE.sin();

  752.             // coordinates of position and velocity in the orbital plane
  753.             final T x      = a.multiply(cosE.subtract(e));
  754.             final T y      = a.multiply(sinE).multiply(s1Me2);
  755.             final T factor = FastMath.sqrt(getMu().divide(a)).divide(e.negate().multiply(cosE).add(1));
  756.             final T xDot   = sinE.negate().multiply(factor);
  757.             final T yDot   = cosE.multiply(s1Me2).multiply(factor);

  758.             final FieldVector3D<T> position = new FieldVector3D<>(x, p, y, q);
  759.             final FieldVector3D<T> velocity = new FieldVector3D<>(xDot, p, yDot, q);
  760.             partialPV = new FieldPVCoordinates<>(position, velocity);

  761.         } else {

  762.             // hyperbolic case

  763.             // compute position and velocity factors
  764.             final FieldSinCos<T> scV = FastMath.sinCos(v);
  765.             final T sinV             = scV.sin();
  766.             final T cosV             = scV.cos();
  767.             final T f                = a.multiply(e.multiply(e).negate().add(1));
  768.             final T posFactor        = f.divide(e.multiply(cosV).add(1));
  769.             final T velFactor        = FastMath.sqrt(getMu().divide(f));

  770.             final FieldVector3D<T> position     = new FieldVector3D<>(posFactor.multiply(cosV), p, posFactor.multiply(sinV), q);
  771.             final FieldVector3D<T> velocity     = new FieldVector3D<>(velFactor.multiply(sinV).negate(), p, velFactor.multiply(e.add(cosV)), q);
  772.             partialPV = new FieldPVCoordinates<>(position, velocity);

  773.         }

  774.     }

  775.     /** Compute non-Keplerian part of the acceleration from first time derivatives.
  776.      * <p>
  777.      * This method should be called only when {@link #hasDerivatives()} returns true.
  778.      * </p>
  779.      * @return non-Keplerian part of the acceleration
  780.      */
  781.     private FieldVector3D<T> nonKeplerianAcceleration() {

  782.         final T[][] dCdP = MathArrays.buildArray(a.getField(), 6, 6);
  783.         getJacobianWrtParameters(PositionAngle.MEAN, dCdP);

  784.         final T nonKeplerianMeanMotion = getMeanAnomalyDot().subtract(getKeplerianMeanMotion());
  785.         final T nonKeplerianAx =     dCdP[3][0].multiply(aDot).
  786.                                  add(dCdP[3][1].multiply(eDot)).
  787.                                  add(dCdP[3][2].multiply(iDot)).
  788.                                  add(dCdP[3][3].multiply(paDot)).
  789.                                  add(dCdP[3][4].multiply(raanDot)).
  790.                                  add(dCdP[3][5].multiply(nonKeplerianMeanMotion));
  791.         final T nonKeplerianAy =     dCdP[4][0].multiply(aDot).
  792.                                  add(dCdP[4][1].multiply(eDot)).
  793.                                  add(dCdP[4][2].multiply(iDot)).
  794.                                  add(dCdP[4][3].multiply(paDot)).
  795.                                  add(dCdP[4][4].multiply(raanDot)).
  796.                                  add(dCdP[4][5].multiply(nonKeplerianMeanMotion));
  797.         final T nonKeplerianAz =     dCdP[5][0].multiply(aDot).
  798.                                  add(dCdP[5][1].multiply(eDot)).
  799.                                  add(dCdP[5][2].multiply(iDot)).
  800.                                  add(dCdP[5][3].multiply(paDot)).
  801.                                  add(dCdP[5][4].multiply(raanDot)).
  802.                                  add(dCdP[5][5].multiply(nonKeplerianMeanMotion));

  803.         return new FieldVector3D<>(nonKeplerianAx, nonKeplerianAy, nonKeplerianAz);

  804.     }

  805.     /** {@inheritDoc} */
  806.     protected TimeStampedFieldPVCoordinates<T> initPVCoordinates() {

  807.         // position and velocity
  808.         computePVWithoutA();

  809.         // acceleration
  810.         final T r2 = partialPV.getPosition().getNormSq();
  811.         final FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r2.multiply(FastMath.sqrt(r2)).reciprocal().multiply(getMu().negate()),
  812.                                                                            partialPV.getPosition());
  813.         final FieldVector3D<T> acceleration = hasDerivatives() ?
  814.                                               keplerianAcceleration.add(nonKeplerianAcceleration()) :
  815.                                               keplerianAcceleration;

  816.         return new TimeStampedFieldPVCoordinates<>(getDate(), partialPV.getPosition(), partialPV.getVelocity(), acceleration);

  817.     }

  818.     /** {@inheritDoc} */
  819.     public FieldKeplerianOrbit<T> shiftedBy(final double dt) {
  820.         return shiftedBy(getDate().getField().getZero().add(dt));
  821.     }

  822.     /** {@inheritDoc} */
  823.     public FieldKeplerianOrbit<T> shiftedBy(final T dt) {

  824.         // use Keplerian-only motion
  825.         final FieldKeplerianOrbit<T> keplerianShifted = new FieldKeplerianOrbit<>(a, e, i, pa, raan,
  826.                                                                                   getKeplerianMeanMotion().multiply(dt).add(getMeanAnomaly()),
  827.                                                                                   PositionAngle.MEAN, getFrame(), getDate().shiftedBy(dt), getMu());

  828.         if (hasDerivatives()) {

  829.             // extract non-Keplerian acceleration from first time derivatives
  830.             final FieldVector3D<T> nonKeplerianAcceleration = nonKeplerianAcceleration();

  831.             // add quadratic effect of non-Keplerian acceleration to Keplerian-only shift
  832.             keplerianShifted.computePVWithoutA();
  833.             final FieldVector3D<T> fixedP   = new FieldVector3D<>(one, keplerianShifted.partialPV.getPosition(),
  834.                                                                   dt.multiply(dt).multiply(0.5), nonKeplerianAcceleration);
  835.             final T   fixedR2 = fixedP.getNormSq();
  836.             final T   fixedR  = fixedR2.sqrt();
  837.             final FieldVector3D<T> fixedV  = new FieldVector3D<>(one, keplerianShifted.partialPV.getVelocity(),
  838.                                                                  dt, nonKeplerianAcceleration);
  839.             final FieldVector3D<T> fixedA  = new FieldVector3D<>(fixedR2.multiply(fixedR).reciprocal().multiply(getMu().negate()),
  840.                                                                  keplerianShifted.partialPV.getPosition(),
  841.                                                                  one, nonKeplerianAcceleration);

  842.             // build a new orbit, taking non-Keplerian acceleration into account
  843.             return new FieldKeplerianOrbit<>(new TimeStampedFieldPVCoordinates<>(keplerianShifted.getDate(),
  844.                                                                                  fixedP, fixedV, fixedA),
  845.                                              keplerianShifted.getFrame(), keplerianShifted.getMu());

  846.         } else {
  847.             // Keplerian-only motion is all we can do
  848.             return keplerianShifted;
  849.         }

  850.     }

  851.     /** {@inheritDoc}
  852.      * <p>
  853.      * The interpolated instance is created by polynomial Hermite interpolation
  854.      * on Keplerian elements, without derivatives (which means the interpolation
  855.      * falls back to Lagrange interpolation only).
  856.      * </p>
  857.      * <p>
  858.      * As this implementation of interpolation is polynomial, it should be used only
  859.      * with small samples (about 10-20 points) in order to avoid <a
  860.      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  861.      * and numerical problems (including NaN appearing).
  862.      * </p>
  863.      * <p>
  864.      * If orbit interpolation on large samples is needed, using the {@link
  865.      * org.orekit.propagation.analytical.Ephemeris} class is a better way than using this
  866.      * low-level interpolation. The Ephemeris class automatically handles selection of
  867.      * a neighboring sub-sample with a predefined number of point from a large global sample
  868.      * in a thread-safe way.
  869.      * </p>
  870.      */
  871.     public FieldKeplerianOrbit<T> interpolate(final FieldAbsoluteDate<T> date, final Stream<FieldOrbit<T>> sample) {

  872.         // first pass to check if derivatives are available throughout the sample
  873.         final List<FieldOrbit<T>> list = sample.collect(Collectors.toList());
  874.         boolean useDerivatives = true;
  875.         for (final FieldOrbit<T> orbit : list) {
  876.             useDerivatives = useDerivatives && orbit.hasDerivatives();
  877.         }

  878.         // set up an interpolator
  879.         final FieldHermiteInterpolator<T> interpolator = new FieldHermiteInterpolator<>();

  880.         // second pass to feed interpolator
  881.         FieldAbsoluteDate<T> previousDate = null;
  882.         T                    previousPA   = zero.add(Double.NaN);
  883.         T                    previousRAAN = zero.add(Double.NaN);
  884.         T                    previousM    = zero.add(Double.NaN);
  885.         for (final FieldOrbit<T> orbit : list) {
  886.             final FieldKeplerianOrbit<T> kep = (FieldKeplerianOrbit<T>) OrbitType.KEPLERIAN.convertType(orbit);
  887.             final T continuousPA;
  888.             final T continuousRAAN;
  889.             final T continuousM;
  890.             if (previousDate == null) {
  891.                 continuousPA   = kep.getPerigeeArgument();
  892.                 continuousRAAN = kep.getRightAscensionOfAscendingNode();
  893.                 continuousM    = kep.getMeanAnomaly();
  894.             } else {
  895.                 final T dt      = kep.getDate().durationFrom(previousDate);
  896.                 final T keplerM = previousM.add(kep.getKeplerianMeanMotion().multiply(dt));
  897.                 continuousPA   = MathUtils.normalizeAngle(kep.getPerigeeArgument(), previousPA);
  898.                 continuousRAAN = MathUtils.normalizeAngle(kep.getRightAscensionOfAscendingNode(), previousRAAN);
  899.                 continuousM    = MathUtils.normalizeAngle(kep.getMeanAnomaly(), keplerM);
  900.             }
  901.             previousDate = kep.getDate();
  902.             previousPA   = continuousPA;
  903.             previousRAAN = continuousRAAN;
  904.             previousM    = continuousM;
  905.             final T[] toAdd = MathArrays.buildArray(getA().getField(), 6);
  906.             toAdd[0] = kep.getA();
  907.             toAdd[1] = kep.getE();
  908.             toAdd[2] = kep.getI();
  909.             toAdd[3] = continuousPA;
  910.             toAdd[4] = continuousRAAN;
  911.             toAdd[5] = continuousM;
  912.             if (useDerivatives) {
  913.                 final T[] toAddDot = MathArrays.buildArray(one.getField(), 6);
  914.                 toAddDot[0] = kep.getADot();
  915.                 toAddDot[1] = kep.getEDot();
  916.                 toAddDot[2] = kep.getIDot();
  917.                 toAddDot[3] = kep.getPerigeeArgumentDot();
  918.                 toAddDot[4] = kep.getRightAscensionOfAscendingNodeDot();
  919.                 toAddDot[5] = kep.getMeanAnomalyDot();
  920.                 interpolator.addSamplePoint(kep.getDate().durationFrom(date),
  921.                                             toAdd, toAddDot);
  922.             } else {
  923.                 interpolator.addSamplePoint(this.zero.add(kep.getDate().durationFrom(date)),
  924.                                             toAdd);
  925.             }
  926.         }

  927.         // interpolate
  928.         final T[][] interpolated = interpolator.derivatives(zero, 1);

  929.         // build a new interpolated instance
  930.         return new FieldKeplerianOrbit<>(interpolated[0][0], interpolated[0][1], interpolated[0][2],
  931.                                          interpolated[0][3], interpolated[0][4], interpolated[0][5],
  932.                                          interpolated[1][0], interpolated[1][1], interpolated[1][2],
  933.                                          interpolated[1][3], interpolated[1][4], interpolated[1][5],
  934.                                          PositionAngle.MEAN, getFrame(), date, getMu());

  935.     }

  936.     /** {@inheritDoc} */
  937.     protected T[][] computeJacobianMeanWrtCartesian() {
  938.         if (a.getReal() > 0) {
  939.             return computeJacobianMeanWrtCartesianElliptical();
  940.         } else {
  941.             return computeJacobianMeanWrtCartesianHyperbolic();
  942.         }
  943.     }

  944.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  945.      * <p>
  946.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  947.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  948.      * yDot for j=4, zDot for j=5).
  949.      * </p>
  950.      * @return 6x6 Jacobian matrix
  951.      */
  952.     private T[][] computeJacobianMeanWrtCartesianElliptical() {

  953.         final T[][] jacobian = MathArrays.buildArray(getA().getField(), 6, 6);

  954.         // compute various intermediate parameters
  955.         computePVWithoutA();
  956.         final FieldVector3D<T> position = partialPV.getPosition();
  957.         final FieldVector3D<T> velocity = partialPV.getVelocity();
  958.         final FieldVector3D<T> momentum = partialPV.getMomentum();
  959.         final T v2         = velocity.getNormSq();
  960.         final T r2         = position.getNormSq();
  961.         final T r          = r2.sqrt();
  962.         final T r3         = r.multiply(r2);

  963.         final T px         = position.getX();
  964.         final T py         = position.getY();
  965.         final T pz         = position.getZ();
  966.         final T vx         = velocity.getX();
  967.         final T vy         = velocity.getY();
  968.         final T vz         = velocity.getZ();
  969.         final T mx         = momentum.getX();
  970.         final T my         = momentum.getY();
  971.         final T mz         = momentum.getZ();

  972.         final T mu         = getMu();
  973.         final T sqrtMuA    = FastMath.sqrt(a.multiply(mu));
  974.         final T sqrtAoMu   = FastMath.sqrt(a.divide(mu));
  975.         final T a2         = a.multiply(a);
  976.         final T twoA       = a.multiply(2);
  977.         final T rOnA       = r.divide(a);

  978.         final T oMe2       = e.multiply(e).negate().add(1);
  979.         final T epsilon    = oMe2.sqrt();
  980.         final T sqrtRec    = epsilon.reciprocal();

  981.         final FieldSinCos<T> scI  = FastMath.sinCos(i);
  982.         final FieldSinCos<T> scPA = FastMath.sinCos(pa);
  983.         final T cosI       = scI.cos();
  984.         final T sinI       = scI.sin();
  985.         final T cosPA      = scPA.cos();
  986.         final T sinPA      = scPA.sin();

  987.         final T pv         = FieldVector3D.dotProduct(position, velocity);
  988.         final T cosE       = a.subtract(r).divide(a.multiply(e));
  989.         final T sinE       = pv.divide(e.multiply(sqrtMuA));

  990.         // da
  991.         final FieldVector3D<T> vectorAR = new FieldVector3D<>(a2.multiply(2).divide(r3), position);
  992.         final FieldVector3D<T> vectorARDot = velocity.scalarMultiply(a2.multiply(mu.divide(2.).reciprocal()));
  993.         fillHalfRow(this.one, vectorAR,    jacobian[0], 0);
  994.         fillHalfRow(this.one, vectorARDot, jacobian[0], 3);

  995.         // de
  996.         final T factorER3 = pv.divide(twoA);
  997.         final FieldVector3D<T> vectorER   = new FieldVector3D<>(cosE.multiply(v2).divide(r.multiply(mu)), position,
  998.                                                                 sinE.divide(sqrtMuA), velocity,
  999.                                                                 factorER3.negate().multiply(sinE).divide(sqrtMuA), vectorAR);
  1000.         final FieldVector3D<T> vectorERDot = new FieldVector3D<>(sinE.divide(sqrtMuA), position,
  1001.                                                                  cosE.multiply(mu.divide(2.).reciprocal()).multiply(r), velocity,
  1002.                                                                  factorER3.negate().multiply(sinE).divide(sqrtMuA), vectorARDot);
  1003.         fillHalfRow(this.one, vectorER,    jacobian[1], 0);
  1004.         fillHalfRow(this.one, vectorERDot, jacobian[1], 3);

  1005.         // dE / dr (Eccentric anomaly)
  1006.         final T coefE = cosE.divide(e.multiply(sqrtMuA));
  1007.         final FieldVector3D<T>  vectorEAnR =
  1008.             new FieldVector3D<>(sinE.negate().multiply(v2).divide(e.multiply(r).multiply(mu)), position, coefE, velocity,
  1009.                                 factorER3.negate().multiply(coefE), vectorAR);

  1010.         // dE / drDot
  1011.         final FieldVector3D<T>  vectorEAnRDot =
  1012.             new FieldVector3D<>(sinE.multiply(-2).multiply(r).divide(e.multiply(mu)), velocity, coefE, position,
  1013.                                 factorER3.negate().multiply(coefE), vectorARDot);

  1014.         // precomputing some more factors
  1015.         final T s1 = sinE.negate().multiply(pz).divide(r).subtract(cosE.multiply(vz).multiply(sqrtAoMu));
  1016.         final T s2 = cosE.negate().multiply(pz).divide(r3);
  1017.         final T s3 = sinE.multiply(vz).divide(sqrtMuA.multiply(-2));
  1018.         final T t1 = sqrtRec.multiply(cosE.multiply(pz).divide(r).subtract(sinE.multiply(vz).multiply(sqrtAoMu)));
  1019.         final T t2 = sqrtRec.multiply(sinE.negate().multiply(pz).divide(r3));
  1020.         final T t3 = sqrtRec.multiply(cosE.subtract(e)).multiply(vz).divide(sqrtMuA.multiply(2));
  1021.         final T t4 = sqrtRec.multiply(e.multiply(sinI).multiply(cosPA).multiply(sqrtRec).subtract(vz.multiply(sqrtAoMu)));
  1022.         final FieldVector3D<T> s = new FieldVector3D<>(cosE.divide(r), this.PLUS_K,
  1023.                                                        s1,       vectorEAnR,
  1024.                                                        s2,       position,
  1025.                                                        s3,       vectorAR);
  1026.         final FieldVector3D<T> sDot = new FieldVector3D<>(sinE.negate().multiply(sqrtAoMu), this.PLUS_K,
  1027.                                                           s1,               vectorEAnRDot,
  1028.                                                           s3,               vectorARDot);
  1029.         final FieldVector3D<T> t =
  1030.             new FieldVector3D<>(sqrtRec.multiply(sinE).divide(r), this.PLUS_K).add(new FieldVector3D<>(t1, vectorEAnR,
  1031.                                                                                                        t2, position,
  1032.                                                                                                        t3, vectorAR,
  1033.                                                                                                        t4, vectorER));
  1034.         final FieldVector3D<T> tDot = new FieldVector3D<>(sqrtRec.multiply(cosE.subtract(e)).multiply(sqrtAoMu), this.PLUS_K,
  1035.                                                           t1,                                                    vectorEAnRDot,
  1036.                                                           t3,                                                    vectorARDot,
  1037.                                                           t4,                                                    vectorERDot);

  1038.         // di
  1039.         final T factorI1 = sinI.negate().multiply(sqrtRec).divide(sqrtMuA);
  1040.         final T i1 =  factorI1;
  1041.         final T i2 =  factorI1.negate().multiply(mz).divide(twoA);
  1042.         final T i3 =  factorI1.multiply(mz).multiply(e).divide(oMe2);
  1043.         final T i4 = cosI.multiply(sinPA);
  1044.         final T i5 = cosI.multiply(cosPA);
  1045.         fillHalfRow(i1, new FieldVector3D<>(vy, vx.negate(), this.zero), i2, vectorAR, i3, vectorER, i4, s, i5, t,
  1046.                     jacobian[2], 0);
  1047.         fillHalfRow(i1, new FieldVector3D<>(py.negate(), px, this.zero), i2, vectorARDot, i3, vectorERDot, i4, sDot, i5, tDot,
  1048.                     jacobian[2], 3);

  1049.         // dpa
  1050.         fillHalfRow(cosPA.divide(sinI), s,    sinPA.negate().divide(sinI), t,    jacobian[3], 0);
  1051.         fillHalfRow(cosPA.divide(sinI), sDot, sinPA.negate().divide(sinI), tDot, jacobian[3], 3);

  1052.         // dRaan
  1053.         final T factorRaanR = (a.multiply(mu).multiply(oMe2).multiply(sinI).multiply(sinI)).reciprocal();
  1054.         fillHalfRow( factorRaanR.negate().multiply(my), new FieldVector3D<>(zero, vz, vy.negate()),
  1055.                      factorRaanR.multiply(mx), new FieldVector3D<>(vz.negate(), zero,  vx),
  1056.                      jacobian[4], 0);
  1057.         fillHalfRow(factorRaanR.negate().multiply(my), new FieldVector3D<>(zero, pz.negate(),  py),
  1058.                      factorRaanR.multiply(mx), new FieldVector3D<>(pz, zero, px.negate()),
  1059.                      jacobian[4], 3);

  1060.         // dM
  1061.         fillHalfRow(rOnA, vectorEAnR,    sinE.negate(), vectorER,    jacobian[5], 0);
  1062.         fillHalfRow(rOnA, vectorEAnRDot, sinE.negate(), vectorERDot, jacobian[5], 3);

  1063.         return jacobian;

  1064.     }

  1065.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1066.      * <p>
  1067.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1068.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1069.      * yDot for j=4, zDot for j=5).
  1070.      * </p>
  1071.      * @return 6x6 Jacobian matrix
  1072.      */
  1073.     private T[][] computeJacobianMeanWrtCartesianHyperbolic() {

  1074.         final T[][] jacobian = MathArrays.buildArray(getA().getField(), 6, 6);

  1075.         // compute various intermediate parameters
  1076.         computePVWithoutA();
  1077.         final FieldVector3D<T> position = partialPV.getPosition();
  1078.         final FieldVector3D<T> velocity = partialPV.getVelocity();
  1079.         final FieldVector3D<T> momentum = partialPV.getMomentum();
  1080.         final T r2         = position.getNormSq();
  1081.         final T r          = r2.sqrt();
  1082.         final T r3         = r.multiply(r2);

  1083.         final T x          = position.getX();
  1084.         final T y          = position.getY();
  1085.         final T z          = position.getZ();
  1086.         final T vx         = velocity.getX();
  1087.         final T vy         = velocity.getY();
  1088.         final T vz         = velocity.getZ();
  1089.         final T mx         = momentum.getX();
  1090.         final T my         = momentum.getY();
  1091.         final T mz         = momentum.getZ();

  1092.         final T mu         = getMu();
  1093.         final T absA       = a.negate();
  1094.         final T sqrtMuA    = absA.multiply(mu).sqrt();
  1095.         final T a2         = a.multiply(a);
  1096.         final T rOa        = r.divide(absA);

  1097.         final FieldSinCos<T> scI = FastMath.sinCos(i);
  1098.         final T cosI       = scI.cos();
  1099.         final T sinI       = scI.sin();

  1100.         final T pv         = FieldVector3D.dotProduct(position, velocity);

  1101.         // da
  1102.         final FieldVector3D<T> vectorAR = new FieldVector3D<>(a2.multiply(-2).divide(r3), position);
  1103.         final FieldVector3D<T> vectorARDot = velocity.scalarMultiply(a2.multiply(-2).divide(mu));
  1104.         fillHalfRow(this.one.negate(), vectorAR,    jacobian[0], 0);
  1105.         fillHalfRow(this.one.negate(), vectorARDot, jacobian[0], 3);

  1106.         // differentials of the momentum
  1107.         final T m      = momentum.getNorm();
  1108.         final T oOm    = m.reciprocal();
  1109.         final FieldVector3D<T> dcXP = new FieldVector3D<>(this.zero, vz, vy.negate());
  1110.         final FieldVector3D<T> dcYP = new FieldVector3D<>(vz.negate(),   this.zero,  vx);
  1111.         final FieldVector3D<T> dcZP = new FieldVector3D<>( vy, vx.negate(),   this.zero);
  1112.         final FieldVector3D<T> dcXV = new FieldVector3D<>(  this.zero,  z.negate(),   y);
  1113.         final FieldVector3D<T> dcYV = new FieldVector3D<>(  z,   this.zero,  x.negate());
  1114.         final FieldVector3D<T> dcZV = new FieldVector3D<>( y.negate(),   x,   this.zero);
  1115.         final FieldVector3D<T> dCP  = new FieldVector3D<>(mx.multiply(oOm), dcXP, my.multiply(oOm), dcYP, mz.multiply(oOm), dcZP);
  1116.         final FieldVector3D<T> dCV  = new FieldVector3D<>(mx.multiply(oOm), dcXV, my.multiply(oOm), dcYV, mz.multiply(oOm), dcZV);

  1117.         // dp
  1118.         final T mOMu   = m.divide(mu);
  1119.         final FieldVector3D<T> dpP  = new FieldVector3D<>(mOMu.multiply(2), dCP);
  1120.         final FieldVector3D<T> dpV  = new FieldVector3D<>(mOMu.multiply(2), dCV);

  1121.         // de
  1122.         final T p      = m.multiply(mOMu);
  1123.         final T moO2ae = absA.multiply(2).multiply(e).reciprocal();
  1124.         final T m2OaMu = p.negate().divide(absA);
  1125.         fillHalfRow(moO2ae, dpP, m2OaMu.multiply(moO2ae), vectorAR,    jacobian[1], 0);
  1126.         fillHalfRow(moO2ae, dpV, m2OaMu.multiply(moO2ae), vectorARDot, jacobian[1], 3);

  1127.         // di
  1128.         final T cI1 = m.multiply(sinI).reciprocal();
  1129.         final T cI2 = cosI.multiply(cI1);
  1130.         fillHalfRow(cI2, dCP, cI1.negate(), dcZP, jacobian[2], 0);
  1131.         fillHalfRow(cI2, dCV, cI1.negate(), dcZV, jacobian[2], 3);


  1132.         // dPA
  1133.         final T cP1     =  y.multiply(oOm);
  1134.         final T cP2     =  x.negate().multiply(oOm);
  1135.         final T cP3     =  mx.multiply(cP1).add(my.multiply(cP2)).negate();
  1136.         final T cP4     =  cP3.multiply(oOm);
  1137.         final T cP5     =  r2.multiply(sinI).multiply(sinI).negate().reciprocal();
  1138.         final T cP6     = z.multiply(cP5);
  1139.         final T cP7     = cP3.multiply(cP5);
  1140.         final FieldVector3D<T> dacP  = new FieldVector3D<>(cP1, dcXP, cP2, dcYP, cP4, dCP, oOm, new FieldVector3D<>(my.negate(), mx, this.zero));
  1141.         final FieldVector3D<T> dacV  = new FieldVector3D<>(cP1, dcXV, cP2, dcYV, cP4, dCV);
  1142.         final FieldVector3D<T> dpoP  = new FieldVector3D<>(cP6, dacP, cP7, this.PLUS_K);
  1143.         final FieldVector3D<T> dpoV  = new FieldVector3D<>(cP6, dacV);

  1144.         final T re2     = r2.multiply(e).multiply(e);
  1145.         final T recOre2 = p.subtract(r).divide(re2);
  1146.         final T resOre2 = pv.multiply(mOMu).divide(re2);
  1147.         final FieldVector3D<T> dreP  = new FieldVector3D<>(mOMu, velocity, pv.divide(mu), dCP);
  1148.         final FieldVector3D<T> dreV  = new FieldVector3D<>(mOMu, position, pv.divide(mu), dCV);
  1149.         final FieldVector3D<T> davP  = new FieldVector3D<>(resOre2.negate(), dpP, recOre2, dreP, resOre2.divide(r), position);
  1150.         final FieldVector3D<T> davV  = new FieldVector3D<>(resOre2.negate(), dpV, recOre2, dreV);
  1151.         fillHalfRow(this.one, dpoP, this.one.negate(), davP, jacobian[3], 0);
  1152.         fillHalfRow(this.one, dpoV, this.one.negate(), davV, jacobian[3], 3);

  1153.         // dRAAN
  1154.         final T cO0 = cI1.multiply(cI1);
  1155.         final T cO1 =  mx.multiply(cO0);
  1156.         final T cO2 =  my.negate().multiply(cO0);
  1157.         fillHalfRow(cO1, dcYP, cO2, dcXP, jacobian[4], 0);
  1158.         fillHalfRow(cO1, dcYV, cO2, dcXV, jacobian[4], 3);

  1159.         // dM
  1160.         final T s2a    = pv.divide(absA.multiply(2));
  1161.         final T oObux  = m.multiply(m).add(absA.multiply(mu)).sqrt().reciprocal();
  1162.         final T scasbu = pv.multiply(oObux);
  1163.         final FieldVector3D<T> dauP = new FieldVector3D<>(sqrtMuA.reciprocal(), velocity, s2a.negate().divide(sqrtMuA), vectorAR);
  1164.         final FieldVector3D<T> dauV = new FieldVector3D<>(sqrtMuA.reciprocal(), position, s2a.negate().divide(sqrtMuA), vectorARDot);
  1165.         final FieldVector3D<T> dbuP = new FieldVector3D<>(oObux.multiply(mu.divide(2.)), vectorAR,    m.multiply(oObux), dCP);
  1166.         final FieldVector3D<T> dbuV = new FieldVector3D<>(oObux.multiply(mu.divide(2.)), vectorARDot, m.multiply(oObux), dCV);
  1167.         final FieldVector3D<T> dcuP = new FieldVector3D<>(oObux, velocity, scasbu.negate().multiply(oObux), dbuP);
  1168.         final FieldVector3D<T> dcuV = new FieldVector3D<>(oObux, position, scasbu.negate().multiply(oObux), dbuV);
  1169.         fillHalfRow(this.one, dauP, e.negate().divide(rOa.add(1)), dcuP, jacobian[5], 0);
  1170.         fillHalfRow(this.one, dauV, e.negate().divide(rOa.add(1)), dcuV, jacobian[5], 3);

  1171.         return jacobian;

  1172.     }

  1173.     /** {@inheritDoc} */
  1174.     protected T[][] computeJacobianEccentricWrtCartesian() {
  1175.         if (a.getReal() > 0) {
  1176.             return computeJacobianEccentricWrtCartesianElliptical();
  1177.         } else {
  1178.             return computeJacobianEccentricWrtCartesianHyperbolic();
  1179.         }
  1180.     }

  1181.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1182.      * <p>
  1183.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1184.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1185.      * yDot for j=4, zDot for j=5).
  1186.      * </p>
  1187.      * @return 6x6 Jacobian matrix
  1188.      */
  1189.     private T[][] computeJacobianEccentricWrtCartesianElliptical() {

  1190.         // start by computing the Jacobian with mean angle
  1191.         final T[][] jacobian = computeJacobianMeanWrtCartesianElliptical();

  1192.         // Differentiating the Kepler equation M = E - e sin E leads to:
  1193.         // dM = (1 - e cos E) dE - sin E de
  1194.         // which is inverted and rewritten as:
  1195.         // dE = a/r dM + sin E a/r de
  1196.         final FieldSinCos<T> scE = FastMath.sinCos(getEccentricAnomaly());
  1197.         final T aOr              = e.negate().multiply(scE.cos()).add(1).reciprocal();

  1198.         // update anomaly row
  1199.         final T[] eRow           = jacobian[1];
  1200.         final T[] anomalyRow     = jacobian[5];
  1201.         for (int j = 0; j < anomalyRow.length; ++j) {
  1202.             anomalyRow[j] = aOr.multiply(anomalyRow[j].add(scE.sin().multiply(eRow[j])));
  1203.         }

  1204.         return jacobian;

  1205.     }

  1206.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1207.      * <p>
  1208.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1209.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1210.      * yDot for j=4, zDot for j=5).
  1211.      * </p>
  1212.      * @return 6x6 Jacobian matrix
  1213.      */
  1214.     private T[][] computeJacobianEccentricWrtCartesianHyperbolic() {

  1215.         // start by computing the Jacobian with mean angle
  1216.         final T[][] jacobian = computeJacobianMeanWrtCartesianHyperbolic();

  1217.         // Differentiating the Kepler equation M = e sinh H - H leads to:
  1218.         // dM = (e cosh H - 1) dH + sinh H de
  1219.         // which is inverted and rewritten as:
  1220.         // dH = 1 / (e cosh H - 1) dM - sinh H / (e cosh H - 1) de
  1221.         final T H      = getEccentricAnomaly();
  1222.         final T coshH  = H.cosh();
  1223.         final T sinhH  = H.sinh();
  1224.         final T absaOr = e.multiply(coshH).subtract(1).reciprocal();
  1225.         // update anomaly row
  1226.         final T[] eRow       = jacobian[1];
  1227.         final T[] anomalyRow = jacobian[5];

  1228.         for (int j = 0; j < anomalyRow.length; ++j) {
  1229.             anomalyRow[j] = absaOr.multiply(anomalyRow[j].subtract(sinhH.multiply(eRow[j])));

  1230.         }

  1231.         return jacobian;

  1232.     }

  1233.     /** {@inheritDoc} */
  1234.     protected T[][] computeJacobianTrueWrtCartesian() {
  1235.         if (a.getReal() > 0) {
  1236.             return computeJacobianTrueWrtCartesianElliptical();
  1237.         } else {
  1238.             return computeJacobianTrueWrtCartesianHyperbolic();
  1239.         }
  1240.     }

  1241.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1242.      * <p>
  1243.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1244.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1245.      * yDot for j=4, zDot for j=5).
  1246.      * </p>
  1247.      * @return 6x6 Jacobian matrix
  1248.      */
  1249.     private T[][] computeJacobianTrueWrtCartesianElliptical() {

  1250.         // start by computing the Jacobian with eccentric angle
  1251.         final T[][] jacobian = computeJacobianEccentricWrtCartesianElliptical();
  1252.         // Differentiating the eccentric anomaly equation sin E = sqrt(1-e^2) sin v / (1 + e cos v)
  1253.         // and using cos E = (e + cos v) / (1 + e cos v) to get rid of cos E leads to:
  1254.         // dE = [sqrt (1 - e^2) / (1 + e cos v)] dv - [sin E / (1 - e^2)] de
  1255.         // which is inverted and rewritten as:
  1256.         // dv = sqrt (1 - e^2) a/r dE + [sin E / sqrt (1 - e^2)] a/r de
  1257.         final T e2               = e.multiply(e);
  1258.         final T oMe2             = e2.negate().add(1);
  1259.         final T epsilon          = oMe2.sqrt();
  1260.         final FieldSinCos<T> scE = FastMath.sinCos(getEccentricAnomaly());
  1261.         final T aOr              = e.multiply(scE.cos()).negate().add(1).reciprocal();
  1262.         final T aFactor          = epsilon.multiply(aOr);
  1263.         final T eFactor          = scE.sin().multiply(aOr).divide(epsilon);

  1264.         // update anomaly row
  1265.         final T[] eRow           = jacobian[1];
  1266.         final T[] anomalyRow     = jacobian[5];
  1267.         for (int j = 0; j < anomalyRow.length; ++j) {
  1268.             anomalyRow[j] = aFactor.multiply(anomalyRow[j]).add(eFactor.multiply(eRow[j]));
  1269.         }
  1270.         return jacobian;

  1271.     }

  1272.     /** Compute the Jacobian of the orbital parameters with respect to the Cartesian parameters.
  1273.      * <p>
  1274.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1275.      * respect to Cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1276.      * yDot for j=4, zDot for j=5).
  1277.      * </p>
  1278.      * @return 6x6 Jacobian matrix
  1279.      */
  1280.     private T[][] computeJacobianTrueWrtCartesianHyperbolic() {

  1281.         // start by computing the Jacobian with eccentric angle
  1282.         final T[][] jacobian = computeJacobianEccentricWrtCartesianHyperbolic();

  1283.         // Differentiating the eccentric anomaly equation sinh H = sqrt(e^2-1) sin v / (1 + e cos v)
  1284.         // and using cosh H = (e + cos v) / (1 + e cos v) to get rid of cosh H leads to:
  1285.         // dH = [sqrt (e^2 - 1) / (1 + e cos v)] dv + [sinh H / (e^2 - 1)] de
  1286.         // which is inverted and rewritten as:
  1287.         // dv = sqrt (1 - e^2) a/r dH - [sinh H / sqrt (e^2 - 1)] a/r de
  1288.         final T e2       = e.multiply(e);
  1289.         final T e2Mo     = e2.subtract(1);
  1290.         final T epsilon  = e2Mo.sqrt();
  1291.         final T H        = getEccentricAnomaly();
  1292.         final T coshH    = H.cosh();
  1293.         final T sinhH    = H.sinh();
  1294.         final T aOr      = e.multiply(coshH).subtract(1).reciprocal();
  1295.         final T aFactor  = epsilon.multiply(aOr);
  1296.         final T eFactor  = sinhH .multiply(aOr).divide(epsilon);

  1297.         // update anomaly row
  1298.         final T[] eRow           = jacobian[1];
  1299.         final T[] anomalyRow     = jacobian[5];
  1300.         for (int j = 0; j < anomalyRow.length; ++j) {
  1301.             anomalyRow[j] = aFactor.multiply(anomalyRow[j]).subtract(eFactor.multiply(eRow[j]));
  1302.         }

  1303.         return jacobian;

  1304.     }

  1305.     /** {@inheritDoc} */
  1306.     public void addKeplerContribution(final PositionAngle type, final T gm,
  1307.                                       final T[] pDot) {
  1308.         final T oMe2;
  1309.         final T ksi;
  1310.         final T absA = a.abs();
  1311.         final T n    = absA.reciprocal().multiply(gm).sqrt().divide(absA);
  1312.         switch (type) {
  1313.             case MEAN :
  1314.                 pDot[5] = pDot[5].add(n);
  1315.                 break;
  1316.             case ECCENTRIC :
  1317.                 oMe2 = e.multiply(e).negate().add(1).abs();
  1318.                 ksi  = e.multiply(v.cos()).add(1);
  1319.                 pDot[5] = pDot[5].add( n.multiply(ksi).divide(oMe2));
  1320.                 break;
  1321.             case TRUE :
  1322.                 oMe2 = e.multiply(e).negate().add(1).abs();
  1323.                 ksi  = e.multiply(v.cos()).add(1);
  1324.                 pDot[5] = pDot[5].add(n.multiply(ksi).multiply(ksi).divide(oMe2.multiply(oMe2.sqrt())));
  1325.                 break;
  1326.             default :
  1327.                 throw new OrekitInternalError(null);
  1328.         }
  1329.     }

  1330.     /**  Returns a string representation of this Keplerian parameters object.
  1331.      * @return a string representation of this object
  1332.      */
  1333.     public String toString() {
  1334.         return new StringBuilder().append("Keplerian parameters: ").append('{').
  1335.                                   append("a: ").append(a.getReal()).
  1336.                                   append("; e: ").append(e.getReal()).
  1337.                                   append("; i: ").append(FastMath.toDegrees(i.getReal())).
  1338.                                   append("; pa: ").append(FastMath.toDegrees(pa.getReal())).
  1339.                                   append("; raan: ").append(FastMath.toDegrees(raan.getReal())).
  1340.                                   append("; v: ").append(FastMath.toDegrees(v.getReal())).
  1341.                                   append(";}").toString();
  1342.     }

  1343.     /** Check if the given parameter is within an acceptable range.
  1344.      * The bounds are inclusive: an exception is raised when either of those conditions are met:
  1345.      * <ul>
  1346.      *     <li>The parameter is strictly greater than upperBound</li>
  1347.      *     <li>The parameter is strictly lower than lowerBound</li>
  1348.      * </ul>
  1349.      * <p>
  1350.      * In either of these cases, an OrekitException is raised.
  1351.      * </p>
  1352.      * @param parameterName name of the parameter
  1353.      * @param parameter value of the parameter
  1354.      * @param lowerBound lower bound of the acceptable range (inclusive)
  1355.      * @param upperBound upper bound of the acceptable range (inclusive)
  1356.      */
  1357.     private void checkParameterRangeInclusive(final String parameterName, final double parameter,
  1358.                                               final double lowerBound, final double upperBound) {
  1359.         if (parameter < lowerBound || parameter > upperBound) {
  1360.             throw new OrekitException(OrekitMessages.INVALID_PARAMETER_RANGE, parameterName,
  1361.                                       parameter, lowerBound, upperBound);
  1362.         }
  1363.     }

  1364.     /** {@inheritDoc} */
  1365.     @Override
  1366.     public KeplerianOrbit toOrbit() {
  1367.         if (hasDerivatives()) {
  1368.             return new KeplerianOrbit(a.getReal(), e.getReal(), i.getReal(),
  1369.                                       pa.getReal(), raan.getReal(), v.getReal(),
  1370.                                       aDot.getReal(), eDot.getReal(), iDot.getReal(),
  1371.                                       paDot.getReal(), raanDot.getReal(), vDot.getReal(),
  1372.                                       PositionAngle.TRUE,
  1373.                                       getFrame(), getDate().toAbsoluteDate(), getMu().getReal());
  1374.         } else {
  1375.             return new KeplerianOrbit(a.getReal(), e.getReal(), i.getReal(),
  1376.                                       pa.getReal(), raan.getReal(), v.getReal(),
  1377.                                       PositionAngle.TRUE,
  1378.                                       getFrame(), getDate().toAbsoluteDate(), getMu().getReal());
  1379.         }
  1380.     }


  1381. }