KeplerianOrbit.java

  1. /* Copyright 2002-2013 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (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;

  19. import java.util.Collection;

  21. import org.apache.commons.math3.analysis.interpolation.HermiteInterpolator;
  22. import org.apache.commons.math3.exception.ConvergenceException;
  23. import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
  24. import org.apache.commons.math3.util.FastMath;
  25. import org.apache.commons.math3.util.MathUtils;
  26. import org.orekit.errors.OrekitException;
  27. import org.orekit.errors.OrekitMessages;
  28. import org.orekit.frames.Frame;
  29. import org.orekit.time.AbsoluteDate;
  30. import org.orekit.utils.PVCoordinates;


  33. /**
  34.  * This class handles traditional keplerian orbital parameters.

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

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

  74.     /** Identifier for mean anomaly.
  75.      * @deprecated as of 6.0 replaced by {@link PositionAngle}
  76.      */
  77.     @Deprecated
  78.     public static final int MEAN_ANOMALY = 0;

  80.     /** Identifier for eccentric anomaly.
  81.      * @deprecated as of 6.0 replaced by {@link PositionAngle}
  82.      */
  83.     @Deprecated
  84.     public static final int ECCENTRIC_ANOMALY = 1;

  86.     /** Identifier for true anomaly.
  87.      * @deprecated as of 6.0 replaced by {@link PositionAngle}
  88.      */
  89.     @Deprecated
  90.     public static final int TRUE_ANOMALY = 2;

  92.     /** Serializable UID. */
  93.     private static final long serialVersionUID = 7593919633854535287L;

  95.     /** First coefficient to compute Kepler equation solver starter. */
  96.     private static final double A;

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

  101.     static {
  102.         final double k1 = 3 * FastMath.PI + 2;
  103.         final double k2 = FastMath.PI - 1;
  104.         final double k3 = 6 * FastMath.PI - 1;
  105.         A  = 3 * k2 * k2 / k1;
  106.         B  = k3 * k3 / (6 * k1);
  107.     }

  109.     /** Semi-major axis (m). */
  110.     private final double a;

  112.     /** Eccentricity. */
  113.     private final double e;

  115.     /** Inclination (rad). */
  116.     private final double i;

  118.     /** Perigee Argument (rad). */
  119.     private final double pa;

  121.     /** Right Ascension of Ascending Node (rad). */
  122.     private final double raan;

  124.     /** True anomaly (rad). */
  125.     private final double v;

  127.     /** Creates a new instance.
  128.      * @param a  semi-major axis (m), negative for hyperbolic orbits
  129.      * @param e eccentricity
  130.      * @param i inclination (rad)
  131.      * @param pa perigee argument (&omega;, rad)
  132.      * @param raan right ascension of ascending node (&Omega;, rad)
  133.      * @param anomaly mean, eccentric or true anomaly (rad)
  134.      * @param type type of anomaly
  135.      * @param frame the frame in which the parameters are defined
  136.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  137.      * @param date date of the orbital parameters
  138.      * @param mu central attraction coefficient (m<sup>3</sup>/s<sup>2</sup>)
  139.      * @exception IllegalArgumentException if frame is not a {@link
  140.      * Frame#isPseudoInertial pseudo-inertial frame} or a and e don't match for hyperbolic orbits,
  141.      * or v is out of range for hyperbolic orbits
  142.      */
  143.     public KeplerianOrbit(final double a, final double e, final double i,
  144.                           final double pa, final double raan,
  145.                           final double anomaly, final PositionAngle type,
  146.                           final Frame frame, final AbsoluteDate date, final double mu)
  147.         throws IllegalArgumentException {
  148.         super(frame, date, mu);

  150.         if (a * (1 - e) < 0) {
  151.             throw OrekitException.createIllegalArgumentException(OrekitMessages.ORBIT_A_E_MISMATCH_WITH_CONIC_TYPE, a, e);
  152.         }

  154.         this.a    =    a;
  155.         this.e    =    e;
  156.         this.i    =    i;
  157.         this.pa   =   pa;
  158.         this.raan = raan;

  160.         final double tmpV;
  161.         switch (type) {
  162.         case MEAN :
  163.             tmpV = (a < 0) ?
  164.                      hyperbolicEccentricToTrue(meanToHyperbolicEccentric(anomaly, e)) :
  165.                      ellipticEccentricToTrue(meanToEllipticEccentric(anomaly));
  166.             break;
  167.         case ECCENTRIC :
  168.             tmpV = (a < 0) ?
  169.                      hyperbolicEccentricToTrue(anomaly) :
  170.                      ellipticEccentricToTrue(anomaly);
  171.             break;
  172.         case TRUE :
  173.             tmpV = anomaly;
  174.             break;
  175.         default : // this should never happen
  176.             throw OrekitException.createInternalError(null);
  177.         }

  179.         // check true anomaly range
  180.         if (1 + e * FastMath.cos(tmpV) <= 0) {
  181.             final double vMax = FastMath.acos(-1 / e);
  182.             throw OrekitException.createIllegalArgumentException(OrekitMessages.ORBIT_ANOMALY_OUT_OF_HYPERBOLIC_RANGE,
  183.                                                                  tmpV, e, -vMax, vMax);
  184.         }
  185.         this.v = tmpV;

  187.     }

  189.     /** Creates a new instance.
  190.      * @param a  semi-major axis (m), negative for hyperbolic orbits
  191.      * @param e eccentricity
  192.      * @param i inclination (rad)
  193.      * @param pa perigee argument (&omega;, rad)
  194.      * @param raan right ascension of ascending node (&Omega;, rad)
  195.      * @param anomaly mean, eccentric or true anomaly (rad)
  196.      * @param type type of anomaly, must be one of {@link #MEAN_ANOMALY},
  197.      * {@link #ECCENTRIC_ANOMALY} or  {@link #TRUE_ANOMALY}
  198.      * @param frame the frame in which the parameters are defined
  199.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  200.      * @param date date of the orbital parameters
  201.      * @param mu central attraction coefficient (m<sup>3</sup>/s<sup>2</sup>)
  202.      * @exception IllegalArgumentException if the longitude argument type is not
  203.      * one of {@link #MEAN_ANOMALY}, {@link #ECCENTRIC_ANOMALY}
  204.      * or {@link #TRUE_ANOMALY} or if frame is not a {@link
  205.      * Frame#isPseudoInertial pseudo-inertial frame}
  206.      * @see #MEAN_ANOMALY
  207.      * @see #ECCENTRIC_ANOMALY
  208.      * @see #TRUE_ANOMALY
  209.      * @deprecated as of 6.0 replaced by {@link #KeplerianOrbit(double, double, double,
  210.      * double, double, double, PositionAngle, Frame, AbsoluteDate, double)}
  211.      * @exception IllegalArgumentException if frame is not a {@link
  212.      * Frame#isPseudoInertial pseudo-inertial frame} or a and e don't match for hyperbolic orbits,
  213.      * or v is out of range for hyperbolic orbits
  214.      */
  215.     @Deprecated
  216.     public KeplerianOrbit(final double a, final double e, final double i,
  217.                           final double pa, final double raan,
  218.                           final double anomaly, final int type,
  219.                           final Frame frame, final AbsoluteDate date, final double mu)
  220.         throws IllegalArgumentException {
  221.         super(frame, date, mu);

  223.         if (a * (1 - e) < 0) {
  224.             throw OrekitException.createIllegalArgumentException(OrekitMessages.ORBIT_A_E_MISMATCH_WITH_CONIC_TYPE, a, e);
  225.         }

  227.         this.a    =    a;
  228.         this.e    =    e;
  229.         this.i    =    i;
  230.         this.pa   =   pa;
  231.         this.raan = raan;

  233.         switch (type) {
  234.         case MEAN_ANOMALY :
  235.             this.v = (a < 0) ?
  236.                      hyperbolicEccentricToTrue(meanToHyperbolicEccentric(anomaly, e)) :
  237.                      ellipticEccentricToTrue(meanToEllipticEccentric(anomaly));
  238.             break;
  239.         case ECCENTRIC_ANOMALY :
  240.             this.v = (a < 0) ?
  241.                      hyperbolicEccentricToTrue(anomaly) :
  242.                      ellipticEccentricToTrue(anomaly);
  243.             break;
  244.         case TRUE_ANOMALY :
  245.             this.v = anomaly;
  246.             break;
  247.         default :
  248.             this.v = Double.NaN;
  249.             throw OrekitException.createIllegalArgumentException(
  250.                   OrekitMessages.ANGLE_TYPE_NOT_SUPPORTED,
  251.                   "MEAN_ANOMALY", "ECCENTRIC_ANOMALY", "TRUE_ANOMALY");

  253.         }
  254.     }

  256.     /** Constructor from cartesian parameters.
  257.      * @param pvCoordinates the PVCoordinates of the satellite
  258.      * @param frame the frame in which are defined the {@link PVCoordinates}
  259.      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
  260.      * @param date date of the orbital parameters
  261.      * @param mu central attraction coefficient (m<sup>3</sup>/s<sup>2</sup>)
  262.      * @exception IllegalArgumentException if frame is not a {@link
  263.      * Frame#isPseudoInertial pseudo-inertial frame}
  264.      */
  265.     public KeplerianOrbit(final PVCoordinates pvCoordinates,
  266.                           final Frame frame, final AbsoluteDate date, final double mu)
  267.         throws IllegalArgumentException {
  268.         super(pvCoordinates, frame, date, mu);

  270.         // compute inclination
  271.         final Vector3D momentum = pvCoordinates.getMomentum();
  272.         final double m2 = momentum.getNormSq();
  273.         i = Vector3D.angle(momentum, Vector3D.PLUS_K);

  275.         // compute right ascension of ascending node
  276.         raan = Vector3D.crossProduct(Vector3D.PLUS_K, momentum).getAlpha();

  278.         // preliminary computations for parameters depending on orbit shape (elliptic or hyperbolic)
  279.         final Vector3D pvP     = pvCoordinates.getPosition();
  280.         final Vector3D pvV     = pvCoordinates.getVelocity();
  281.         final double   r       = pvP.getNorm();
  282.         final double   V2      = pvV.getNormSq();
  283.         final double   rV2OnMu = r * V2 / mu;

  285.         // compute semi-major axis (will be negative for hyperbolic orbits)
  286.         a = r / (2 - rV2OnMu);
  287.         final double muA = mu * a;

  289.         // compute true anomaly
  290.         if (a > 0) {
  291.             // elliptic or circular orbit
  292.             final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(muA);
  293.             final double eCE = rV2OnMu - 1;
  294.             e = FastMath.sqrt(eSE * eSE + eCE * eCE);
  295.             v = ellipticEccentricToTrue(FastMath.atan2(eSE, eCE));
  296.         } else {
  297.             // hyperbolic orbit
  298.             final double eSH = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(-muA);
  299.             final double eCH = rV2OnMu - 1;
  300.             e = FastMath.sqrt(1 - m2 / muA);
  301.             v = hyperbolicEccentricToTrue(FastMath.log((eCH + eSH) / (eCH - eSH)) / 2);
  302.         }

  304.         // compute perigee argument
  305.         final Vector3D node = new Vector3D(raan, 0.0);
  306.         final double px = Vector3D.dotProduct(pvP, node);
  307.         final double py = Vector3D.dotProduct(pvP, Vector3D.crossProduct(momentum, node)) / FastMath.sqrt(m2);
  308.         pa = FastMath.atan2(py, px) - v;

  310.     }

  312.     /** Constructor from any kind of orbital parameters.
  313.      * @param op orbital parameters to copy
  314.      */
  315.     public KeplerianOrbit(final Orbit op) {
  316.         this(op.getPVCoordinates(), op.getFrame(), op.getDate(), op.getMu());
  317.     }

  319.     /** {@inheritDoc} */
  320.     public OrbitType getType() {
  321.         return OrbitType.KEPLERIAN;
  322.     }

  324.     /** {@inheritDoc} */
  325.     public double getA() {
  326.         return a;
  327.     }

  329.     /** {@inheritDoc} */
  330.     public double getE() {
  331.         return e;
  332.     }

  334.     /** {@inheritDoc} */
  335.     public double getI() {
  336.         return i;
  337.     }

  339.     /** Get the perigee argument.
  340.      * @return perigee argument (rad)
  341.      */
  342.     public double getPerigeeArgument() {
  343.         return pa;
  344.     }

  346.     /** Get the right ascension of the ascending node.
  347.      * @return right ascension of the ascending node (rad)
  348.      */
  349.     public double getRightAscensionOfAscendingNode() {
  350.         return raan;
  351.     }

  353.     /** Get the anomaly.
  354.      * @param type type of the angle
  355.      * @return anomaly (rad)
  356.      */
  357.     public double getAnomaly(final PositionAngle type) {
  358.         return (type == PositionAngle.MEAN) ? getMeanAnomaly() :
  359.                                               ((type == PositionAngle.ECCENTRIC) ? getEccentricAnomaly() :
  360.                                                                                    getTrueAnomaly());
  361.     }

  363.     /** Get the true anomaly.
  364.      * @return true anomaly (rad)
  365.      */
  366.     public double getTrueAnomaly() {
  367.         return v;
  368.     }

  370.     /** Get the eccentric anomaly.
  371.      * @return eccentric anomaly (rad)
  372.      */
  373.     public double getEccentricAnomaly() {
  374.         if (a < 0) {
  375.             // hyperbolic case
  376.             final double sinhH = FastMath.sqrt(e * e - 1) * FastMath.sin(v) /
  377.                                  (1 + e * FastMath.cos(v));
  378.             return FastMath.asinh(sinhH);
  379.         }

  381.         // elliptic case
  382.         final double beta = e / (1 + FastMath.sqrt((1 - e) * (1 + e)));
  383.         return v - 2 * FastMath.atan(beta * FastMath.sin(v) / (1 + beta * FastMath.cos(v)));

  385.     }

  387.     /** Computes the true anomaly from the elliptic eccentric anomaly.
  388.      * @param E eccentric anomaly (rad)
  389.      * @return v the true anomaly
  390.      */
  391.     private double ellipticEccentricToTrue(final double E) {
  392.         final double beta = e / (1 + FastMath.sqrt((1 - e) * (1 + e)));
  393.         return E + 2 * FastMath.atan(beta * FastMath.sin(E) / (1 - beta * FastMath.cos(E)));
  394.     }

  396.     /** Computes the true anomaly from the hyperbolic eccentric anomaly.
  397.      * @param H hyperbolic eccentric anomaly (rad)
  398.      * @return v the true anomaly
  399.      */
  400.     private double hyperbolicEccentricToTrue(final double H) {
  401.         return 2 * FastMath.atan(FastMath.sqrt((e + 1) / (e - 1)) * FastMath.tanh(H / 2));
  402.     }

  404.     /** Get the mean anomaly.
  405.      * @return mean anomaly (rad)
  406.      */
  407.     public double getMeanAnomaly() {

  409.         if (a < 0) {
  410.             // hyperbolic case
  411.             final double H = getEccentricAnomaly();
  412.             return e * FastMath.sinh(H) - H;
  413.         }

  415.         // elliptic case
  416.         final double E = getEccentricAnomaly();
  417.         return E - e * FastMath.sin(E);

  419.     }

  421.     /** Computes the elliptic eccentric anomaly from the mean anomaly.
  422.      * <p>
  423.      * The algorithm used here for solving Kepler equation has been published
  424.      * in: "Procedures for  solving Kepler's Equation", A. W. Odell and
  425.      * R. H. Gooding, Celestial Mechanics 38 (1986) 307-334
  426.      * </p>
  427.      * @param M mean anomaly (rad)
  428.      * @return v the true anomaly
  429.      */
  430.     private double meanToEllipticEccentric(final double M) {

  432.         // reduce M to [-PI PI) interval
  433.         final double reducedM = MathUtils.normalizeAngle(M, 0.0);

  435.         // compute start value according to A. W. Odell and R. H. Gooding S12 starter
  436.         double E;
  437.         if (FastMath.abs(reducedM) < 1.0 / 6.0) {
  438.             E = reducedM + e * (FastMath.cbrt(6 * reducedM) - reducedM);
  439.         } else {
  440.             if (reducedM < 0) {
  441.                 final double w = FastMath.PI + reducedM;
  442.                 E = reducedM + e * (A * w / (B - w) - FastMath.PI - reducedM);
  443.             } else {
  444.                 final double w = FastMath.PI - reducedM;
  445.                 E = reducedM + e * (FastMath.PI - A * w / (B - w) - reducedM);
  446.             }
  447.         }

  449.         final double e1 = 1 - e;
  450.         final boolean noCancellationRisk = (e1 + E * E / 6) >= 0.1;

  452.         // perform two iterations, each consisting of one Halley step and one Newton-Raphson step
  453.         for (int j = 0; j < 2; ++j) {
  454.             double f;
  455.             double fd;
  456.             final double fdd  = e * FastMath.sin(E);
  457.             final double fddd = e * FastMath.cos(E);
  458.             if (noCancellationRisk) {
  459.                 f  = (E - fdd) - reducedM;
  460.                 fd = 1 - fddd;
  461.             } else {
  462.                 f  = eMeSinE(E) - reducedM;
  463.                 final double s = FastMath.sin(0.5 * E);
  464.                 fd = e1 + 2 * e * s * s;
  465.             }
  466.             final double dee = f * fd / (0.5 * f * fdd - fd * fd);

  468.             // update eccentric anomaly, using expressions that limit underflow problems
  469.             final double w = fd + 0.5 * dee * (fdd + dee * fddd / 3);
  470.             fd += dee * (fdd + 0.5 * dee * fddd);
  471.             E  -= (f - dee * (fd - w)) / fd;

  473.         }

  475.         // expand the result back to original range
  476.         E += M - reducedM;

  478.         return E;

  480.     }

  482.     /** Accurate computation of E - e sin(E).
  483.      * <p>
  484.      * This method is used when E is close to 0 and e close to 1,
  485.      * i.e. near the perigee of almost parabolic orbits
  486.      * </p>
  487.      * @param E eccentric anomaly
  488.      * @return E - e sin(E)
  489.      */
  490.     private double eMeSinE(final double E) {
  491.         double x = (1 - e) * FastMath.sin(E);
  492.         final double mE2 = -E * E;
  493.         double term = E;
  494.         double d    = 0;
  495.         // the inequality test below IS intentional and should NOT be replaced by a check with a small tolerance
  496.         for (double x0 = Double.NaN; x != x0;) {
  497.             d += 2;
  498.             term *= mE2 / (d * (d + 1));
  499.             x0 = x;
  500.             x = x - term;
  501.         }
  502.         return x;
  503.     }

  505.     /** Computes the hyperbolic eccentric anomaly from the mean anomaly.
  506.      * <p>
  507.      * The algorithm used here for solving hyperbolic Kepler equation is
  508.      * Danby's iterative method (3rd order) with Vallado's initial guess.
  509.      * </p>
  510.      * @param M mean anomaly (rad)
  511.      * @param ecc eccentricity
  512.      * @return H the hyperbolic eccentric anomaly
  513.      */
  514.     private double meanToHyperbolicEccentric(final double M, final double ecc) {

  516.         // Resolution of hyperbolic Kepler equation for keplerian parameters

  518.         // Initial guess
  519.         double H;
  520.         if (ecc < 1.6) {
  521.             if ((-FastMath.PI < M && M < 0.) || M > FastMath.PI) {
  522.                 H = M - ecc;
  523.             } else {
  524.                 H = M + ecc;
  525.             }
  526.         } else {
  527.             if (ecc < 3.6 && FastMath.abs(M) > FastMath.PI) {
  528.                 H = M - FastMath.copySign(ecc, M);
  529.             } else {
  530.                 H = M / (ecc - 1.);
  531.             }
  532.         }

  534.         // Iterative computation
  535.         int iter = 0;
  536.         do {
  537.             final double f3  = ecc * FastMath.cosh(H);
  538.             final double f2  = ecc * FastMath.sinh(H);
  539.             final double f1  = f3 - 1.;
  540.             final double f0  = f2 - H - M;
  541.             final double f12 = 2. * f1;
  542.             final double d   = f0 / f12;
  543.             final double fdf = f1 - d * f2;
  544.             final double ds  = f0 / fdf;

  546.             final double shift = f0 / (fdf + ds * ds * f3 / 6.);

  548.             H -= shift;

  550.             if (FastMath.abs(shift) <= 1.0e-12) {
  551.                 return H;
  552.             }

  554.         } while (++iter < 50);

  556.         throw new ConvergenceException(OrekitMessages.UNABLE_TO_COMPUTE_HYPERBOLIC_ECCENTRIC_ANOMALY,
  557.                                        iter);
  558.     }

  560.     /** {@inheritDoc} */
  561.     public double getEquinoctialEx() {
  562.         return  e * FastMath.cos(pa + raan);
  563.     }

  565.     /** {@inheritDoc} */
  566.     public double getEquinoctialEy() {
  567.         return  e * FastMath.sin(pa + raan);
  568.     }

  570.     /** {@inheritDoc} */
  571.     public double getHx() {
  572.         // Check for equatorial retrograde orbit
  573.         if (FastMath.abs(i - FastMath.PI) < 1.0e-10) {
  574.             return Double.NaN;
  575.         }
  576.         return  FastMath.cos(raan) * FastMath.tan(i / 2);
  577.     }

  579.     /** {@inheritDoc} */
  580.     public double getHy() {
  581.         // Check for equatorial retrograde orbit
  582.         if (FastMath.abs(i - FastMath.PI) < 1.0e-10) {
  583.             return Double.NaN;
  584.         }
  585.         return  FastMath.sin(raan) * FastMath.tan(i / 2);
  586.     }

  588.     /** {@inheritDoc} */
  589.     public double getLv() {
  590.         return pa + raan + v;
  591.     }

  593.     /** {@inheritDoc} */
  594.     public double getLE() {
  595.         return pa + raan + getEccentricAnomaly();
  596.     }

  598.     /** {@inheritDoc} */
  599.     public double getLM() {
  600.         return pa + raan + getMeanAnomaly();
  601.     }

  603.     /** {@inheritDoc} */
  604.     protected PVCoordinates initPVCoordinates() {

  606.         // preliminary variables
  607.         final double cosRaan = FastMath.cos(raan);
  608.         final double sinRaan = FastMath.sin(raan);
  609.         final double cosPa   = FastMath.cos(pa);
  610.         final double sinPa   = FastMath.sin(pa);
  611.         final double cosI    = FastMath.cos(i);
  612.         final double sinI    = FastMath.sin(i);

  614.         final double crcp    = cosRaan * cosPa;
  615.         final double crsp    = cosRaan * sinPa;
  616.         final double srcp    = sinRaan * cosPa;
  617.         final double srsp    = sinRaan * sinPa;

  619.         // reference axes defining the orbital plane
  620.         final Vector3D p = new Vector3D( crcp - cosI * srsp,  srcp + cosI * crsp, sinI * sinPa);
  621.         final Vector3D q = new Vector3D(-crsp - cosI * srcp, -srsp + cosI * crcp, sinI * cosPa);

  623.         return (a > 0) ? initPVCoordinatesElliptical(p, q) : initPVCoordinatesHyperbolic(p, q);

  625.     }

  627.     /** Initialize the position/velocity coordinates, elliptic case.
  628.      * @param p unit vector in the orbital plane pointing towards perigee
  629.      * @param q unit vector in the orbital plane in quadrature with q
  630.      * @return computed position/velocity coordinates
  631.      */
  632.     private PVCoordinates initPVCoordinatesElliptical(final Vector3D p, final Vector3D q) {

  634.         // elliptic eccentric anomaly
  635.         final double uME2   = (1 - e) * (1 + e);
  636.         final double s1Me2  = FastMath.sqrt(uME2);
  637.         final double E      = getEccentricAnomaly();
  638.         final double cosE   = FastMath.cos(E);
  639.         final double sinE   = FastMath.sin(E);

  641.         // coordinates of position and velocity in the orbital plane
  642.         final double x      = a * (cosE - e);
  643.         final double y      = a * sinE * s1Me2;
  644.         final double factor = FastMath.sqrt(getMu() / a) / (1 - e * cosE);
  645.         final double xDot   = -sinE * factor;
  646.         final double yDot   =  cosE * s1Me2 * factor;

  648.         return new PVCoordinates(new Vector3D(x, p, y, q), new Vector3D(xDot, p, yDot, q));

  650.     }

  652.     /** Initialize the position/velocity coordinates, hyperbolic case.
  653.      * @param p unit vector in the orbital plane pointing towards perigee
  654.      * @param q unit vector in the orbital plane in quadrature with q
  655.      * @return computed position/velocity coordinates
  656.      */
  657.     private PVCoordinates initPVCoordinatesHyperbolic(final Vector3D p, final Vector3D q) {

  659.         // compute position and velocity factors
  660.         final double sinV      = FastMath.sin(v);
  661.         final double cosV      = FastMath.cos(v);
  662.         final double f         = a * (1 - e * e);
  663.         final double posFactor = f / (1 + e * cosV);
  664.         final double velFactor = FastMath.sqrt(getMu() / f);

  666.         return new PVCoordinates(new Vector3D( posFactor * cosV, p, posFactor * sinV, q),
  667.                                  new Vector3D(-velFactor * sinV, p, velFactor * (e + cosV), q));

  669.     }

  671.     /** {@inheritDoc} */
  672.     public KeplerianOrbit shiftedBy(final double dt) {
  673.         return new KeplerianOrbit(a, e, i, pa, raan,
  674.                                   getMeanAnomaly() + getKeplerianMeanMotion() * dt,
  675.                                   PositionAngle.MEAN, getFrame(), getDate().shiftedBy(dt), getMu());
  676.     }

  678.     /** {@inheritDoc}
  679.      * <p>
  680.      * The interpolated instance is created by polynomial Hermite interpolation
  681.      * on Keplerian elements, without derivatives (which means the interpolation
  682.      * falls back to Lagrange interpolation only).
  683.      * </p>
  684.      * <p>
  685.      * As this implementation of interpolation is polynomial, it should be used only
  686.      * with small samples (about 10-20 points) in order to avoid <a
  687.      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
  688.      * and numerical problems (including NaN appearing).
  689.      * </p>
  690.      * <p>
  691.      * If orbit interpolation on large samples is needed, using the {@link
  692.      * org.orekit.propagation.analytical.Ephemeris} class is a better way than using this
  693.      * low-level interpolation. The Ephemeris class automatically handles selection of
  694.      * a neighboring sub-sample with a predefined number of point from a large global sample
  695.      * in a thread-safe way.
  696.      * </p>
  697.      */
  698.     public KeplerianOrbit interpolate(final AbsoluteDate date, final Collection<Orbit> sample) {

  700.         // set up an interpolator
  701.         final HermiteInterpolator interpolator = new HermiteInterpolator();

  703.         // add sample points
  704.         AbsoluteDate previousDate = null;
  705.         double previousPA   = Double.NaN;
  706.         double previousRAAN = Double.NaN;
  707.         double previousM    = Double.NaN;
  708.         for (final Orbit orbit : sample) {
  709.             final KeplerianOrbit kep = (KeplerianOrbit) OrbitType.KEPLERIAN.convertType(orbit);
  710.             final double continuousPA;
  711.             final double continuousRAAN;
  712.             final double continuousM;
  713.             if (previousDate == null) {
  714.                 continuousPA   = kep.getPerigeeArgument();
  715.                 continuousRAAN = kep.getRightAscensionOfAscendingNode();
  716.                 continuousM    = kep.getMeanAnomaly();
  717.             } else {
  718.                 final double dt      = kep.getDate().durationFrom(previousDate);
  719.                 final double keplerM = previousM + kep.getKeplerianMeanMotion() * dt;
  720.                 continuousPA   = MathUtils.normalizeAngle(kep.getPerigeeArgument(), previousPA);
  721.                 continuousRAAN = MathUtils.normalizeAngle(kep.getRightAscensionOfAscendingNode(), previousRAAN);
  722.                 continuousM    = MathUtils.normalizeAngle(kep.getMeanAnomaly(), keplerM);
  723.             }
  724.             previousDate = kep.getDate();
  725.             previousPA   = continuousPA;
  726.             previousRAAN = continuousRAAN;
  727.             previousM    = continuousM;
  728.             interpolator.addSamplePoint(kep.getDate().durationFrom(date),
  729.                                         new double[] {
  730.                                             kep.getA(),
  731.                                             kep.getE(),
  732.                                             kep.getI(),
  733.                                             continuousPA,
  734.                                             continuousRAAN,
  735.                                             continuousM
  736.                                         });
  737.         }

  739.         // interpolate
  740.         final double[] interpolated = interpolator.value(0);

  742.         // build a new interpolated instance
  743.         return new KeplerianOrbit(interpolated[0], interpolated[1], interpolated[2],
  744.                                   interpolated[3], interpolated[4], interpolated[5],
  745.                                   PositionAngle.MEAN, getFrame(), date, getMu());

  747.     }

  749.     /** {@inheritDoc} */
  750.     protected double[][] computeJacobianMeanWrtCartesian() {
  751.         if (a > 0) {
  752.             return computeJacobianMeanWrtCartesianElliptical();
  753.         } else {
  754.             return computeJacobianMeanWrtCartesianHyperbolic();
  755.         }
  756.     }

  758.     /** Compute the Jacobian of the orbital parameters with respect to the cartesian parameters.
  759.      * <p>
  760.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  761.      * respect to cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  762.      * yDot for j=4, zDot for j=5).
  763.      * </p>
  764.      * @return 6x6 Jacobian matrix
  765.      */
  766.     private double[][] computeJacobianMeanWrtCartesianElliptical() {

  768.         final double[][] jacobian = new double[6][6];

  770.         // compute various intermediate parameters
  771.         final PVCoordinates pvc = getPVCoordinates();
  772.         final Vector3D position = pvc.getPosition();
  773.         final Vector3D velocity = pvc.getVelocity();
  774.         final Vector3D momentum = pvc.getMomentum();
  775.         final double v2         = velocity.getNormSq();
  776.         final double r2         = position.getNormSq();
  777.         final double r          = FastMath.sqrt(r2);
  778.         final double r3         = r * r2;

  780.         final double px         = position.getX();
  781.         final double py         = position.getY();
  782.         final double pz         = position.getZ();
  783.         final double vx         = velocity.getX();
  784.         final double vy         = velocity.getY();
  785.         final double vz         = velocity.getZ();
  786.         final double mx         = momentum.getX();
  787.         final double my         = momentum.getY();
  788.         final double mz         = momentum.getZ();

  790.         final double mu         = getMu();
  791.         final double sqrtMuA    = FastMath.sqrt(a * mu);
  792.         final double sqrtAoMu   = FastMath.sqrt(a / mu);
  793.         final double a2         = a * a;
  794.         final double twoA       = 2 * a;
  795.         final double rOnA       = r / a;

  797.         final double oMe2       = 1 - e * e;
  798.         final double epsilon    = FastMath.sqrt(oMe2);
  799.         final double sqrtRec    = 1 / epsilon;

  801.         final double cosI       = FastMath.cos(i);
  802.         final double sinI       = FastMath.sin(i);
  803.         final double cosPA      = FastMath.cos(pa);
  804.         final double sinPA      = FastMath.sin(pa);

  806.         final double pv         = Vector3D.dotProduct(position, velocity);
  807.         final double cosE       = (a - r) / (a * e);
  808.         final double sinE       = pv / (e * sqrtMuA);

  810.         // da
  811.         final Vector3D vectorAR = new Vector3D(2 * a2 / r3, position);
  812.         final Vector3D vectorARDot = velocity.scalarMultiply(2 * a2 / mu);
  813.         fillHalfRow(1, vectorAR,    jacobian[0], 0);
  814.         fillHalfRow(1, vectorARDot, jacobian[0], 3);

  816.         // de
  817.         final double factorER3 = pv / twoA;
  818.         final Vector3D vectorER   = new Vector3D(cosE * v2 / (r * mu), position,
  819.                                                  sinE / sqrtMuA, velocity,
  820.                                                  -factorER3 * sinE / sqrtMuA, vectorAR);
  821.         final Vector3D vectorERDot = new Vector3D(sinE / sqrtMuA, position,
  822.                                                   cosE * 2 * r / mu, velocity,
  823.                                                   -factorER3 * sinE / sqrtMuA, vectorARDot);
  824.         fillHalfRow(1, vectorER,    jacobian[1], 0);
  825.         fillHalfRow(1, vectorERDot, jacobian[1], 3);

  827.         // dE / dr (Eccentric anomaly)
  828.         final double coefE = cosE / (e * sqrtMuA);
  829.         final Vector3D  vectorEAnR =
  830.             new Vector3D(-sinE * v2 / (e * r * mu), position, coefE, velocity,
  831.                          -factorER3 * coefE, vectorAR);

  833.         // dE / drDot
  834.         final Vector3D  vectorEAnRDot =
  835.             new Vector3D(-sinE * 2 * r / (e * mu), velocity, coefE, position,
  836.                          -factorER3 * coefE, vectorARDot);

  838.         // precomputing some more factors
  839.         final double s1 = -sinE * pz / r - cosE * vz * sqrtAoMu;
  840.         final double s2 = -cosE * pz / r3;
  841.         final double s3 = -sinE * vz / (2 * sqrtMuA);
  842.         final double t1 = sqrtRec * (cosE * pz / r - sinE * vz * sqrtAoMu);
  843.         final double t2 = sqrtRec * (-sinE * pz / r3);
  844.         final double t3 = sqrtRec * (cosE - e) * vz / (2 * sqrtMuA);
  845.         final double t4 = sqrtRec * (e * sinI * cosPA * sqrtRec - vz * sqrtAoMu);
  846.         final Vector3D s = new Vector3D(cosE / r, Vector3D.PLUS_K,
  847.                                         s1,       vectorEAnR,
  848.                                         s2,       position,
  849.                                         s3,       vectorAR);
  850.         final Vector3D sDot = new Vector3D(-sinE * sqrtAoMu, Vector3D.PLUS_K,
  851.                                            s1,               vectorEAnRDot,
  852.                                            s3,               vectorARDot);
  853.         final Vector3D t =
  854.             new Vector3D(sqrtRec * sinE / r, Vector3D.PLUS_K).add(new Vector3D(t1, vectorEAnR,
  855.                                                                                t2, position,
  856.                                                                                t3, vectorAR,
  857.                                                                                t4, vectorER));
  858.         final Vector3D tDot = new Vector3D(sqrtRec * (cosE - e) * sqrtAoMu, Vector3D.PLUS_K,
  859.                                            t1,                              vectorEAnRDot,
  860.                                            t3,                              vectorARDot,
  861.                                            t4,                              vectorERDot);

  863.         // di
  864.         final double factorI1 = -sinI * sqrtRec / sqrtMuA;
  865.         final double i1 =  factorI1;
  866.         final double i2 = -factorI1 * mz / twoA;
  867.         final double i3 =  factorI1 * mz * e / oMe2;
  868.         final double i4 = cosI * sinPA;
  869.         final double i5 = cosI * cosPA;
  870.         fillHalfRow(i1, new Vector3D(vy, -vx, 0), i2, vectorAR, i3, vectorER, i4, s, i5, t,
  871.                     jacobian[2], 0);
  872.         fillHalfRow(i1, new Vector3D(-py, px, 0), i2, vectorARDot, i3, vectorERDot, i4, sDot, i5, tDot,
  873.                     jacobian[2], 3);

  875.         // dpa
  876.         fillHalfRow(cosPA / sinI, s,    -sinPA / sinI, t,    jacobian[3], 0);
  877.         fillHalfRow(cosPA / sinI, sDot, -sinPA / sinI, tDot, jacobian[3], 3);

  879.         // dRaan
  880.         final double factorRaanR = 1 / (mu * a * oMe2 * sinI * sinI);
  881.         fillHalfRow(-factorRaanR * my, new Vector3D(  0, vz, -vy),
  882.                      factorRaanR * mx, new Vector3D(-vz,  0,  vx),
  883.                      jacobian[4], 0);
  884.         fillHalfRow(-factorRaanR * my, new Vector3D( 0, -pz,  py),
  885.                      factorRaanR * mx, new Vector3D(pz,   0, -px),
  886.                      jacobian[4], 3);

  888.         // dM
  889.         fillHalfRow(rOnA, vectorEAnR,    -sinE, vectorER,    jacobian[5], 0);
  890.         fillHalfRow(rOnA, vectorEAnRDot, -sinE, vectorERDot, jacobian[5], 3);

  892.         return jacobian;

  894.     }

  896.     /** Compute the Jacobian of the orbital parameters with respect to the cartesian parameters.
  897.      * <p>
  898.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  899.      * respect to cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  900.      * yDot for j=4, zDot for j=5).
  901.      * </p>
  902.      * @return 6x6 Jacobian matrix
  903.      */
  904.     private double[][] computeJacobianMeanWrtCartesianHyperbolic() {

  906.         final double[][] jacobian = new double[6][6];

  908.         // compute various intermediate parameters
  909.         final PVCoordinates pvc = getPVCoordinates();
  910.         final Vector3D position = pvc.getPosition();
  911.         final Vector3D velocity = pvc.getVelocity();
  912.         final Vector3D momentum = pvc.getMomentum();
  913.         final double r2         = position.getNormSq();
  914.         final double r          = FastMath.sqrt(r2);
  915.         final double r3         = r * r2;

  917.         final double x          = position.getX();
  918.         final double y          = position.getY();
  919.         final double z          = position.getZ();
  920.         final double vx         = velocity.getX();
  921.         final double vy         = velocity.getY();
  922.         final double vz         = velocity.getZ();
  923.         final double mx         = momentum.getX();
  924.         final double my         = momentum.getY();
  925.         final double mz         = momentum.getZ();

  927.         final double mu         = getMu();
  928.         final double absA       = -a;
  929.         final double sqrtMuA    = FastMath.sqrt(absA * mu);
  930.         final double a2         = a * a;
  931.         final double rOa        = r / absA;

  933.         final double cosI       = FastMath.cos(i);
  934.         final double sinI       = FastMath.sin(i);

  936.         final double pv         = Vector3D.dotProduct(position, velocity);

  938.         // da
  939.         final Vector3D vectorAR = new Vector3D(-2 * a2 / r3, position);
  940.         final Vector3D vectorARDot = velocity.scalarMultiply(-2 * a2 / mu);
  941.         fillHalfRow(-1, vectorAR,    jacobian[0], 0);
  942.         fillHalfRow(-1, vectorARDot, jacobian[0], 3);

  944.         // differentials of the momentum
  945.         final double m      = momentum.getNorm();
  946.         final double oOm    = 1 / m;
  947.         final Vector3D dcXP = new Vector3D(  0,  vz, -vy);
  948.         final Vector3D dcYP = new Vector3D(-vz,   0,  vx);
  949.         final Vector3D dcZP = new Vector3D( vy, -vx,   0);
  950.         final Vector3D dcXV = new Vector3D(  0,  -z,   y);
  951.         final Vector3D dcYV = new Vector3D(  z,   0,  -x);
  952.         final Vector3D dcZV = new Vector3D( -y,   x,   0);
  953.         final Vector3D dCP  = new Vector3D(mx * oOm, dcXP, my * oOm, dcYP, mz * oOm, dcZP);
  954.         final Vector3D dCV  = new Vector3D(mx * oOm, dcXV, my * oOm, dcYV, mz * oOm, dcZV);

  956.         // dp
  957.         final double mOMu   = m / mu;
  958.         final Vector3D dpP  = new Vector3D(2 * mOMu, dCP);
  959.         final Vector3D dpV  = new Vector3D(2 * mOMu, dCV);

  961.         // de
  962.         final double p      = m * mOMu;
  963.         final double moO2ae = 1 / (2 * absA * e);
  964.         final double m2OaMu = -p / absA;
  965.         fillHalfRow(moO2ae, dpP, m2OaMu * moO2ae, vectorAR,    jacobian[1], 0);
  966.         fillHalfRow(moO2ae, dpV, m2OaMu * moO2ae, vectorARDot, jacobian[1], 3);

  968.         // di
  969.         final double cI1 = 1 / (m * sinI);
  970.         final double cI2 = cosI * cI1;
  971.         fillHalfRow(cI2, dCP, -cI1, dcZP, jacobian[2], 0);
  972.         fillHalfRow(cI2, dCV, -cI1, dcZV, jacobian[2], 3);

  974.         // dPA
  975.         final double cP1     =  y * oOm;
  976.         final double cP2     = -x * oOm;
  977.         final double cP3     = -(mx * cP1 + my * cP2);
  978.         final double cP4     = cP3 * oOm;
  979.         final double cP5     = -1 / (r2 * sinI * sinI);
  980.         final double cP6     = z  * cP5;
  981.         final double cP7     = cP3 * cP5;
  982.         final Vector3D dacP  = new Vector3D(cP1, dcXP, cP2, dcYP, cP4, dCP, oOm, new Vector3D(-my, mx, 0));
  983.         final Vector3D dacV  = new Vector3D(cP1, dcXV, cP2, dcYV, cP4, dCV);
  984.         final Vector3D dpoP  = new Vector3D(cP6, dacP, cP7, Vector3D.PLUS_K);
  985.         final Vector3D dpoV  = new Vector3D(cP6, dacV);

  987.         final double re2     = r2 * e * e;
  988.         final double recOre2 = (p - r) / re2;
  989.         final double resOre2 = (pv * mOMu) / re2;
  990.         final Vector3D dreP  = new Vector3D(mOMu, velocity, pv / mu, dCP);
  991.         final Vector3D dreV  = new Vector3D(mOMu, position, pv / mu, dCV);
  992.         final Vector3D davP  = new Vector3D(-resOre2, dpP, recOre2, dreP, resOre2 / r, position);
  993.         final Vector3D davV  = new Vector3D(-resOre2, dpV, recOre2, dreV);
  994.         fillHalfRow(1, dpoP, -1, davP, jacobian[3], 0);
  995.         fillHalfRow(1, dpoV, -1, davV, jacobian[3], 3);

  997.         // dRAAN
  998.         final double cO0 = cI1 * cI1;
  999.         final double cO1 =  mx * cO0;
  1000.         final double cO2 = -my * cO0;
  1001.         fillHalfRow(cO1, dcYP, cO2, dcXP, jacobian[4], 0);
  1002.         fillHalfRow(cO1, dcYV, cO2, dcXV, jacobian[4], 3);

  1004.         // dM
  1005.         final double s2a    = pv / (2 * absA);
  1006.         final double oObux  = 1 / FastMath.sqrt(m * m + mu * absA);
  1007.         final double scasbu = pv * oObux;
  1008.         final Vector3D dauP = new Vector3D(1 / sqrtMuA, velocity, -s2a / sqrtMuA, vectorAR);
  1009.         final Vector3D dauV = new Vector3D(1 / sqrtMuA, position, -s2a / sqrtMuA, vectorARDot);
  1010.         final Vector3D dbuP = new Vector3D(oObux * mu / 2, vectorAR,    m * oObux, dCP);
  1011.         final Vector3D dbuV = new Vector3D(oObux * mu / 2, vectorARDot, m * oObux, dCV);
  1012.         final Vector3D dcuP = new Vector3D(oObux, velocity, -scasbu * oObux, dbuP);
  1013.         final Vector3D dcuV = new Vector3D(oObux, position, -scasbu * oObux, dbuV);
  1014.         fillHalfRow(1, dauP, -e / (1 + rOa), dcuP, jacobian[5], 0);
  1015.         fillHalfRow(1, dauV, -e / (1 + rOa), dcuV, jacobian[5], 3);

  1017.         return jacobian;

  1019.     }

  1021.     /** {@inheritDoc} */
  1022.     protected double[][] computeJacobianEccentricWrtCartesian() {
  1023.         if (a > 0) {
  1024.             return computeJacobianEccentricWrtCartesianElliptical();
  1025.         } else {
  1026.             return computeJacobianEccentricWrtCartesianHyperbolic();
  1027.         }
  1028.     }

  1030.     /** Compute the Jacobian of the orbital parameters with respect to the cartesian parameters.
  1031.      * <p>
  1032.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1033.      * respect to cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1034.      * yDot for j=4, zDot for j=5).
  1035.      * </p>
  1036.      * @return 6x6 Jacobian matrix
  1037.      */
  1038.     private double[][] computeJacobianEccentricWrtCartesianElliptical() {

  1040.         // start by computing the Jacobian with mean angle
  1041.         final double[][] jacobian = computeJacobianMeanWrtCartesianElliptical();

  1043.         // Differentiating the Kepler equation M = E - e sin E leads to:
  1044.         // dM = (1 - e cos E) dE - sin E de
  1045.         // which is inverted and rewritten as:
  1046.         // dE = a/r dM + sin E a/r de
  1047.         final double eccentricAnomaly = getEccentricAnomaly();
  1048.         final double cosE             = FastMath.cos(eccentricAnomaly);
  1049.         final double sinE             = FastMath.sin(eccentricAnomaly);
  1050.         final double aOr              = 1 / (1 - e * cosE);

  1052.         // update anomaly row
  1053.         final double[] eRow           = jacobian[1];
  1054.         final double[] anomalyRow     = jacobian[5];
  1055.         for (int j = 0; j < anomalyRow.length; ++j) {
  1056.             anomalyRow[j] = aOr * (anomalyRow[j] + sinE * eRow[j]);
  1057.         }

  1059.         return jacobian;

  1061.     }

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

  1073.         // start by computing the Jacobian with mean angle
  1074.         final double[][] jacobian = computeJacobianMeanWrtCartesianHyperbolic();

  1076.         // Differentiating the Kepler equation M = e sinh H - H leads to:
  1077.         // dM = (e cosh H - 1) dH + sinh H de
  1078.         // which is inverted and rewritten as:
  1079.         // dH = 1 / (e cosh H - 1) dM - sinh H / (e cosh H - 1) de
  1080.         final double H      = getEccentricAnomaly();
  1081.         final double coshH  = FastMath.cosh(H);
  1082.         final double sinhH  = FastMath.sinh(H);
  1083.         final double absaOr = 1 / (e * coshH - 1);

  1085.         // update anomaly row
  1086.         final double[] eRow       = jacobian[1];
  1087.         final double[] anomalyRow = jacobian[5];
  1088.         for (int j = 0; j < anomalyRow.length; ++j) {
  1089.             anomalyRow[j] = absaOr * (anomalyRow[j] - sinhH * eRow[j]);
  1090.         }

  1092.         return jacobian;

  1094.     }

  1096.     /** {@inheritDoc} */
  1097.     protected double[][] computeJacobianTrueWrtCartesian() {
  1098.         if (a > 0) {
  1099.             return computeJacobianTrueWrtCartesianElliptical();
  1100.         } else {
  1101.             return computeJacobianTrueWrtCartesianHyperbolic();
  1102.         }
  1103.     }

  1105.     /** Compute the Jacobian of the orbital parameters with respect to the cartesian parameters.
  1106.      * <p>
  1107.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1108.      * respect to cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1109.      * yDot for j=4, zDot for j=5).
  1110.      * </p>
  1111.      * @return 6x6 Jacobian matrix
  1112.      */
  1113.     private double[][] computeJacobianTrueWrtCartesianElliptical() {

  1115.         // start by computing the Jacobian with eccentric angle
  1116.         final double[][] jacobian = computeJacobianEccentricWrtCartesianElliptical();

  1118.         // Differentiating the eccentric anomaly equation sin E = sqrt(1-e^2) sin v / (1 + e cos v)
  1119.         // and using cos E = (e + cos v) / (1 + e cos v) to get rid of cos E leads to:
  1120.         // dE = [sqrt (1 - e^2) / (1 + e cos v)] dv - [sin E / (1 - e^2)] de
  1121.         // which is inverted and rewritten as:
  1122.         // dv = sqrt (1 - e^2) a/r dE + [sin E / sqrt (1 - e^2)] a/r de
  1123.         final double e2               = e * e;
  1124.         final double oMe2             = 1 - e2;
  1125.         final double epsilon          = FastMath.sqrt(oMe2);
  1126.         final double eccentricAnomaly = getEccentricAnomaly();
  1127.         final double cosE             = FastMath.cos(eccentricAnomaly);
  1128.         final double sinE             = FastMath.sin(eccentricAnomaly);
  1129.         final double aOr              = 1 / (1 - e * cosE);
  1130.         final double aFactor          = epsilon * aOr;
  1131.         final double eFactor          = sinE * aOr / epsilon;

  1133.         // update anomaly row
  1134.         final double[] eRow           = jacobian[1];
  1135.         final double[] anomalyRow     = jacobian[5];
  1136.         for (int j = 0; j < anomalyRow.length; ++j) {
  1137.             anomalyRow[j] = aFactor * anomalyRow[j] + eFactor * eRow[j];
  1138.         }

  1140.         return jacobian;

  1142.     }

  1144.     /** Compute the Jacobian of the orbital parameters with respect to the cartesian parameters.
  1145.      * <p>
  1146.      * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with
  1147.      * respect to cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3,
  1148.      * yDot for j=4, zDot for j=5).
  1149.      * </p>
  1150.      * @return 6x6 Jacobian matrix
  1151.      */
  1152.     private double[][] computeJacobianTrueWrtCartesianHyperbolic() {

  1154.         // start by computing the Jacobian with eccentric angle
  1155.         final double[][] jacobian = computeJacobianEccentricWrtCartesianHyperbolic();

  1157.         // Differentiating the eccentric anomaly equation sinh H = sqrt(e^2-1) sin v / (1 + e cos v)
  1158.         // and using cosh H = (e + cos v) / (1 + e cos v) to get rid of cosh H leads to:
  1159.         // dH = [sqrt (e^2 - 1) / (1 + e cos v)] dv + [sinh H / (e^2 - 1)] de
  1160.         // which is inverted and rewritten as:
  1161.         // dv = sqrt (1 - e^2) a/r dH - [sinh H / sqrt (e^2 - 1)] a/r de
  1162.         final double e2       = e * e;
  1163.         final double e2Mo     = e2 - 1;
  1164.         final double epsilon  = FastMath.sqrt(e2Mo);
  1165.         final double H        = getEccentricAnomaly();
  1166.         final double coshH    = FastMath.cosh(H);
  1167.         final double sinhH    = FastMath.sinh(H);
  1168.         final double aOr      = 1 / (e * coshH - 1);
  1169.         final double aFactor  = epsilon * aOr;
  1170.         final double eFactor  = sinhH * aOr / epsilon;

  1172.         // update anomaly row
  1173.         final double[] eRow           = jacobian[1];
  1174.         final double[] anomalyRow     = jacobian[5];
  1175.         for (int j = 0; j < anomalyRow.length; ++j) {
  1176.             anomalyRow[j] = aFactor * anomalyRow[j] - eFactor * eRow[j];
  1177.         }

  1179.         return jacobian;

  1181.     }

  1183.     /** {@inheritDoc} */
  1184.     public void addKeplerContribution(final PositionAngle type, final double gm,
  1185.                                       final double[] pDot) {
  1186.         final double oMe2;
  1187.         final double ksi;
  1188.         final double absA = FastMath.abs(a);
  1189.         final double n    = FastMath.sqrt(gm / absA) / absA;
  1190.         switch (type) {
  1191.         case MEAN :
  1192.             pDot[5] += n;
  1193.             break;
  1194.         case ECCENTRIC :
  1195.             oMe2 = FastMath.abs(1 - e * e);
  1196.             ksi  = 1 + e * FastMath.cos(v);
  1197.             pDot[5] += n * ksi / oMe2;
  1198.             break;
  1199.         case TRUE :
  1200.             oMe2 = FastMath.abs(1 - e * e);
  1201.             ksi  = 1 + e * FastMath.cos(v);
  1202.             pDot[5] += n * ksi * ksi / (oMe2 * FastMath.sqrt(oMe2));
  1203.             break;
  1204.         default :
  1205.             throw OrekitException.createInternalError(null);
  1206.         }
  1207.     }

  1209.     /**  Returns a string representation of this keplerian parameters object.
  1210.      * @return a string representation of this object
  1211.      */
  1212.     public String toString() {
  1213.         return new StringBuffer().append("keplerian parameters: ").append('{').
  1214.                                   append("a: ").append(a).
  1215.                                   append("; e: ").append(e).
  1216.                                   append("; i: ").append(FastMath.toDegrees(i)).
  1217.                                   append("; pa: ").append(FastMath.toDegrees(pa)).
  1218.                                   append("; raan: ").append(FastMath.toDegrees(raan)).
  1219.                                   append("; v: ").append(FastMath.toDegrees(v)).
  1220.                                   append(";}").toString();
  1221.     }

  1223. }
Created with JaCoCo 0.6.3.201306030806