FieldEcksteinHechlerPropagator.java

  1. /* Copyright 2002-2019 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.propagation.analytical;

  18. import org.hipparchus.Field;
  19. import org.hipparchus.RealFieldElement;
  20. import org.hipparchus.analysis.differentiation.FDSFactory;
  21. import org.hipparchus.analysis.differentiation.FieldDerivativeStructure;
  22. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  23. import org.hipparchus.util.FastMath;
  24. import org.hipparchus.util.MathArrays;
  25. import org.orekit.attitudes.AttitudeProvider;
  26. import org.orekit.errors.OrekitException;
  27. import org.orekit.errors.OrekitMessages;
  28. import org.orekit.forces.gravity.potential.UnnormalizedSphericalHarmonicsProvider;
  29. import org.orekit.orbits.FieldCartesianOrbit;
  30. import org.orekit.orbits.FieldCircularOrbit;
  31. import org.orekit.orbits.FieldOrbit;
  32. import org.orekit.orbits.OrbitType;
  33. import org.orekit.orbits.PositionAngle;
  34. import org.orekit.propagation.FieldSpacecraftState;
  35. import org.orekit.time.FieldAbsoluteDate;
  36. import org.orekit.utils.FieldTimeSpanMap;
  37. import org.orekit.utils.TimeStampedFieldPVCoordinates;

  38. /** This class propagates a {@link org.orekit.propagation.FieldSpacecraftState}
  39.  *  using the analytical Eckstein-Hechler model.
  40.  * <p>The Eckstein-Hechler model is suited for near circular orbits
  41.  * (e &lt; 0.1, with poor accuracy between 0.005 and 0.1) and inclination
  42.  * neither equatorial (direct or retrograde) nor critical (direct or
  43.  * retrograde).</p>
  44.  * @see FieldOrbit
  45.  * @author Guylaine Prat
  46.  */
  47. public class FieldEcksteinHechlerPropagator<T extends RealFieldElement<T>> extends FieldAbstractAnalyticalPropagator<T> {

  48.     /** Factory for the derivatives. */
  49.     private final FDSFactory<T> factory;

  50.     /** Initial Eckstein-Hechler model. */
  51.     private FieldEHModel<T> initialModel;

  52.     /** All models. */
  53.     private transient FieldTimeSpanMap<FieldEHModel<T>, T> models;

  54.     /** Reference radius of the central body attraction model (m). */
  55.     private double referenceRadius;

  56.     /** Central attraction coefficient (m³/s²). */
  57.     private double mu;

  58.     /** Un-normalized zonal coefficients. */
  59.     private double[] ck0;

  60.     /** Build a propagator from FieldOrbit<T> and potential provider.
  61.      * <p>Mass and attitude provider are set to unspecified non-null arbitrary values.</p>
  62.      * @param initialOrbit initial FieldOrbit<T>
  63.      * @param provider for un-normalized zonal coefficients
  64.      */
  65.     public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
  66.                                           final UnnormalizedSphericalHarmonicsProvider provider) {
  67.         this(initialOrbit, DEFAULT_LAW, initialOrbit.getA().getField().getZero().add(DEFAULT_MASS), provider,
  68.              provider.onDate(initialOrbit.getDate().toAbsoluteDate()));
  69.     }

  70.     /**
  71.      * Private helper constructor.
  72.      * @param initialOrbit initial FieldOrbit<T>
  73.      * @param attitude attitude provider
  74.      * @param mass spacecraft mass
  75.      * @param provider for un-normalized zonal coefficients
  76.      * @param harmonics {@code provider.onDate(initialOrbit.getDate())}
  77.      */
  78.     public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
  79.                                           final  AttitudeProvider attitude,
  80.                                           final T mass,
  81.                                           final UnnormalizedSphericalHarmonicsProvider provider,
  82.                                           final UnnormalizedSphericalHarmonicsProvider.UnnormalizedSphericalHarmonics harmonics) {
  83.         this(initialOrbit, attitude,  mass, provider.getAe(), provider.getMu(),
  84.              harmonics.getUnnormalizedCnm(2, 0),
  85.              harmonics.getUnnormalizedCnm(3, 0),
  86.              harmonics.getUnnormalizedCnm(4, 0),
  87.              harmonics.getUnnormalizedCnm(5, 0),
  88.              harmonics.getUnnormalizedCnm(6, 0));
  89.     }

  90.     /** Build a propagator from FieldOrbit<T> and potential.
  91.      * <p>Mass and attitude provider are set to unspecified non-null arbitrary values.</p>
  92.      * <p>The C<sub>n,0</sub> coefficients are the denormalized zonal coefficients, they
  93.      * are related to both the normalized coefficients
  94.      * <span style="text-decoration: overline">C</span><sub>n,0</sub>
  95.      *  and the J<sub>n</sub> one as follows:</p>
  96.      * <pre>
  97.      *   C<sub>n,0</sub> = [(2-δ<sub>0,m</sub>)(2n+1)(n-m)!/(n+m)!]<sup>½</sup>
  98.      *                      <span style="text-decoration: overline">C</span><sub>n,0</sub>
  99.      *   C<sub>n,0</sub> = -J<sub>n</sub>
  100.      * </pre>
  101.      * @param initialOrbit initial FieldOrbit<T>
  102.      * @param referenceRadius reference radius of the Earth for the potential model (m)
  103.      * @param mu central attraction coefficient (m³/s²)
  104.      * @param c20 un-normalized zonal coefficient (about -1.08e-3 for Earth)
  105.      * @param c30 un-normalized zonal coefficient (about +2.53e-6 for Earth)
  106.      * @param c40 un-normalized zonal coefficient (about +1.62e-6 for Earth)
  107.      * @param c50 un-normalized zonal coefficient (about +2.28e-7 for Earth)
  108.      * @param c60 un-normalized zonal coefficient (about -5.41e-7 for Earth)
  109.           * @see org.orekit.utils.Constants
  110.      */
  111.     public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
  112.                                           final double referenceRadius, final double mu,
  113.                                           final double c20, final double c30, final double c40,
  114.                                           final double c50, final double c60) {
  115.         this(initialOrbit, DEFAULT_LAW, initialOrbit.getDate().getField().getZero().add(DEFAULT_MASS),
  116.              referenceRadius, mu, c20, c30, c40, c50, c60);
  117.     }

  118.     /** Build a propagator from FieldOrbit<T>, mass and potential provider.
  119.      * <p>Attitude law is set to an unspecified non-null arbitrary value.</p>
  120.      * @param initialOrbit initial FieldOrbit<T>
  121.      * @param mass spacecraft mass
  122.      * @param provider for un-normalized zonal coefficients
  123.      */
  124.     public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit, final T mass,
  125.                                           final UnnormalizedSphericalHarmonicsProvider provider) {
  126.         this(initialOrbit, DEFAULT_LAW, mass, provider, provider.onDate(initialOrbit.getDate().toAbsoluteDate()));
  127.     }

  128.     /** Build a propagator from FieldOrbit<T>, mass and potential.
  129.      * <p>Attitude law is set to an unspecified non-null arbitrary value.</p>
  130.      * <p>The C<sub>n,0</sub> coefficients are the denormalized zonal coefficients, they
  131.      * are related to both the normalized coefficients
  132.      * <span style="text-decoration: overline">C</span><sub>n,0</sub>
  133.      *  and the J<sub>n</sub> one as follows:</p>
  134.      * <pre>
  135.      *   C<sub>n,0</sub> = [(2-δ<sub>0,m</sub>)(2n+1)(n-m)!/(n+m)!]<sup>½</sup>
  136.      *                      <span style="text-decoration: overline">C</span><sub>n,0</sub>
  137.      *   C<sub>n,0</sub> = -J<sub>n</sub>
  138.      * </pre>
  139.      * @param initialOrbit initial FieldOrbit<T>
  140.      * @param mass spacecraft mass
  141.      * @param referenceRadius reference radius of the Earth for the potential model (m)
  142.      * @param mu central attraction coefficient (m³/s²)
  143.      * @param c20 un-normalized zonal coefficient (about -1.08e-3 for Earth)
  144.      * @param c30 un-normalized zonal coefficient (about +2.53e-6 for Earth)
  145.      * @param c40 un-normalized zonal coefficient (about +1.62e-6 for Earth)
  146.      * @param c50 un-normalized zonal coefficient (about +2.28e-7 for Earth)
  147.      * @param c60 un-normalized zonal coefficient (about -5.41e-7 for Earth)
  148.      */
  149.     public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit, final T mass,
  150.                                           final double referenceRadius, final double mu,
  151.                                           final double c20, final double c30, final double c40,
  152.                                           final double c50, final double c60) {
  153.         this(initialOrbit, DEFAULT_LAW, mass, referenceRadius, mu, c20, c30, c40, c50, c60);
  154.     }

  155.     /** Build a propagator from FieldOrbit<T>, attitude provider and potential provider.
  156.      * <p>Mass is set to an unspecified non-null arbitrary value.</p>
  157.      * @param initialOrbit initial FieldOrbit<T>
  158.      * @param attitudeProv attitude provider
  159.      * @param provider for un-normalized zonal coefficients
  160.      */
  161.     public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
  162.                                           final AttitudeProvider attitudeProv,
  163.                                           final UnnormalizedSphericalHarmonicsProvider provider) {
  164.         this(initialOrbit, attitudeProv, initialOrbit.getA().getField().getZero().add(DEFAULT_MASS), provider, provider.onDate(initialOrbit.getDate().toAbsoluteDate()));
  165.     }

  166.     /** Build a propagator from FieldOrbit<T>, attitude provider and potential.
  167.      * <p>Mass is set to an unspecified non-null arbitrary value.</p>
  168.      * <p>The C<sub>n,0</sub> coefficients are the denormalized zonal coefficients, they
  169.      * are related to both the normalized coefficients
  170.      * <span style="text-decoration: overline">C</span><sub>n,0</sub>
  171.      *  and the J<sub>n</sub> one as follows:</p>
  172.      * <pre>
  173.      *   C<sub>n,0</sub> = [(2-δ<sub>0,m</sub>)(2n+1)(n-m)!/(n+m)!]<sup>½</sup>
  174.      *                     <span style="text-decoration: overline">C</span><sub>n,0</sub>
  175.      *   C<sub>n,0</sub> = -J<sub>n</sub>
  176.      * </pre>
  177.      * @param initialOrbit initial FieldOrbit<T>
  178.      * @param attitudeProv attitude provider
  179.      * @param referenceRadius reference radius of the Earth for the potential model (m)
  180.      * @param mu central attraction coefficient (m³/s²)
  181.      * @param c20 un-normalized zonal coefficient (about -1.08e-3 for Earth)
  182.      * @param c30 un-normalized zonal coefficient (about +2.53e-6 for Earth)
  183.      * @param c40 un-normalized zonal coefficient (about +1.62e-6 for Earth)
  184.      * @param c50 un-normalized zonal coefficient (about +2.28e-7 for Earth)
  185.      * @param c60 un-normalized zonal coefficient (about -5.41e-7 for Earth)
  186.      */
  187.     public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
  188.                                           final AttitudeProvider attitudeProv,
  189.                                           final double referenceRadius, final double mu,
  190.                                           final double c20, final double c30, final double c40,
  191.                                           final double c50, final double c60) {
  192.         this(initialOrbit, attitudeProv, initialOrbit.getDate().getField().getZero().add(DEFAULT_MASS),
  193.              referenceRadius, mu, c20, c30, c40, c50, c60);
  194.     }

  195.     /** Build a propagator from FieldOrbit<T>, attitude provider, mass and potential provider.
  196.      * @param initialOrbit initial FieldOrbit<T>
  197.      * @param attitudeProv attitude provider
  198.      * @param mass spacecraft mass
  199.      * @param provider for un-normalized zonal coefficients
  200.      */
  201.     public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
  202.                                           final AttitudeProvider attitudeProv,
  203.                                           final T mass,
  204.                                           final UnnormalizedSphericalHarmonicsProvider provider) {
  205.         this(initialOrbit, attitudeProv, mass, provider, provider.onDate(initialOrbit.getDate().toAbsoluteDate()));
  206.     }

  207.     /** Build a propagator from FieldOrbit<T>, attitude provider, mass and potential.
  208.      * <p>The C<sub>n,0</sub> coefficients are the denormalized zonal coefficients, they
  209.      * are related to both the normalized coefficients
  210.      * <span style="text-decoration: overline">C</span><sub>n,0</sub>
  211.      *  and the J<sub>n</sub> one as follows:</p>
  212.      * <pre>
  213.      *   C<sub>n,0</sub> = [(2-δ<sub>0,m</sub>)(2n+1)(n-m)!/(n+m)!]<sup>½</sup>
  214.      *                      <span style="text-decoration: overline">C</span><sub>n,0</sub>
  215.      *   C<sub>n,0</sub> = -J<sub>n</sub>
  216.      * </pre>
  217.      * @param initialOrbit initial FieldOrbit<T>
  218.      * @param attitudeProv attitude provider
  219.      * @param mass spacecraft mass
  220.      * @param referenceRadius reference radius of the Earth for the potential model (m)
  221.      * @param mu central attraction coefficient (m³/s²)
  222.      * @param c20 un-normalized zonal coefficient (about -1.08e-3 for Earth)
  223.      * @param c30 un-normalized zonal coefficient (about +2.53e-6 for Earth)
  224.      * @param c40 un-normalized zonal coefficient (about +1.62e-6 for Earth)
  225.      * @param c50 un-normalized zonal coefficient (about +2.28e-7 for Earth)
  226.      * @param c60 un-normalized zonal coefficient (about -5.41e-7 for Earth)
  227.      */
  228.     public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
  229.                                           final AttitudeProvider attitudeProv,
  230.                                           final T mass,
  231.                                           final double referenceRadius, final double mu,
  232.                                           final double c20, final double c30, final double c40,
  233.                                           final double c50, final double c60) {

  234.         super(mass.getField(), attitudeProv);
  235.         final Field<T> field = mass.getField();
  236.         factory = new FDSFactory<>(field, 1, 2);
  237.         try {

  238.             // store model coefficients
  239.             this.referenceRadius = referenceRadius;
  240.             this.mu  = mu;
  241.             this.ck0 = new double[] {
  242.                 0.0, 0.0, c20, c30, c40, c50, c60
  243.             };

  244.             // compute mean parameters
  245.             // transform into circular adapted parameters used by the Eckstein-Hechler model
  246.             resetInitialState(new FieldSpacecraftState<>(initialOrbit,
  247.                                                          attitudeProv.getAttitude(initialOrbit,
  248.                                                                                   initialOrbit.getDate(),
  249.                                                                                   initialOrbit.getFrame()),
  250.                                                          mass));

  251.         } catch (OrekitException oe) {
  252.             throw new OrekitException(oe);
  253.         }
  254.     }

  255.     /** {@inheritDoc} */
  256.     public void resetInitialState(final FieldSpacecraftState<T> state) {
  257.         super.resetInitialState(state);
  258.         this.initialModel = computeMeanParameters((FieldCircularOrbit<T>) OrbitType.CIRCULAR.convertType(state.getOrbit()),
  259.                                                   state.getMass());
  260.         this.models       = new FieldTimeSpanMap<FieldEHModel<T>, T>(initialModel, state.getA().getField());
  261.     }

  262.     /** {@inheritDoc} */
  263.     protected void resetIntermediateState(final FieldSpacecraftState<T> state, final boolean forward) {
  264.         final FieldEHModel<T> newModel = computeMeanParameters((FieldCircularOrbit<T>) OrbitType.CIRCULAR.convertType(state.getOrbit()),
  265.                                                        state.getMass());
  266.         if (forward) {
  267.             models.addValidAfter(newModel, state.getDate());
  268.         } else {
  269.             models.addValidBefore(newModel, state.getDate());
  270.         }
  271.     }

  272.     /** Compute mean parameters according to the Eckstein-Hechler analytical model.
  273.      * @param osculating osculating FieldOrbit<T>
  274.      * @param mass constant mass
  275.      * @return Eckstein-Hechler mean model
  276.      */
  277.     private FieldEHModel<T> computeMeanParameters(final FieldCircularOrbit<T> osculating, final T mass) {

  278.         // sanity check
  279.         if (osculating.getA().getReal() < referenceRadius) {
  280.             throw new OrekitException(OrekitMessages.TRAJECTORY_INSIDE_BRILLOUIN_SPHERE,
  281.                                            osculating.getA());
  282.         }
  283.         final Field<T> field = mass.getField();
  284.         final T one = field.getOne();
  285.         final T zero = field.getZero();
  286.         // rough initialization of the mean parameters
  287.         FieldEHModel<T> current = new FieldEHModel<>(factory, osculating, mass, referenceRadius, mu, ck0);
  288.         // threshold for each parameter
  289.         final T epsilon         = one .multiply(1.0e-13);
  290.         final T thresholdA      = epsilon.multiply(current.mean.getA().abs().add(1.0));
  291.         final T thresholdE      = epsilon.multiply(current.mean.getE().add(1.0));
  292.         final T thresholdAngles = epsilon.multiply(FastMath.PI);


  293.         int i = 0;
  294.         while (i++ < 100) {

  295.             // recompute the osculating parameters from the current mean parameters
  296.             final FieldDerivativeStructure<T>[] parameters = current.propagateParameters(current.mean.getDate());
  297.             // adapted parameters residuals
  298.             final T deltaA      = osculating.getA()         .subtract(parameters[0].getValue());
  299.             final T deltaEx     = osculating.getCircularEx().subtract(parameters[1].getValue());
  300.             final T deltaEy     = osculating.getCircularEy().subtract(parameters[2].getValue());
  301.             final T deltaI      = osculating.getI()         .subtract(parameters[3].getValue());
  302.             final T deltaRAAN   = normalizeAngle(osculating.getRightAscensionOfAscendingNode().subtract(
  303.                                                                 parameters[4].getValue()),
  304.                                                                 zero);
  305.             final T deltaAlphaM = normalizeAngle(osculating.getAlphaM().subtract(parameters[5].getValue()), zero);
  306.             // update mean parameters
  307.             current = new FieldEHModel<>(factory,
  308.                                          new FieldCircularOrbit<>(current.mean.getA().add(deltaA),
  309.                                                                   current.mean.getCircularEx().add( deltaEx),
  310.                                                                   current.mean.getCircularEy().add( deltaEy),
  311.                                                                   current.mean.getI()         .add( deltaI ),
  312.                                                                   current.mean.getRightAscensionOfAscendingNode().add(deltaRAAN),
  313.                                                                   current.mean.getAlphaM().add(deltaAlphaM),
  314.                                                                   PositionAngle.MEAN,
  315.                                                                   current.mean.getFrame(),
  316.                                                                   current.mean.getDate(), mu),
  317.                                   mass, referenceRadius, mu, ck0);
  318.             // check convergence
  319.             if ((FastMath.abs(deltaA.getReal())      < thresholdA.getReal()) &&
  320.                 (FastMath.abs(deltaEx.getReal())     < thresholdE.getReal()) &&
  321.                 (FastMath.abs(deltaEy.getReal())     < thresholdE.getReal()) &&
  322.                 (FastMath.abs(deltaI.getReal())      < thresholdAngles.getReal()) &&
  323.                 (FastMath.abs(deltaRAAN.getReal())   < thresholdAngles.getReal()) &&
  324.                 (FastMath.abs(deltaAlphaM.getReal()) < thresholdAngles.getReal())) {
  325.                 return current;
  326.             }

  327.         }

  328.         throw new OrekitException(OrekitMessages.UNABLE_TO_COMPUTE_ECKSTEIN_HECHLER_MEAN_PARAMETERS, i);

  329.     }

  330.     /** {@inheritDoc} */
  331.     public FieldCartesianOrbit<T> propagateOrbit(final FieldAbsoluteDate<T> date) {
  332.         // compute Cartesian parameters, taking derivatives into account
  333.         // to make sure velocity and acceleration are consistent
  334.         final FieldEHModel<T> current = models.get(date);
  335.         return new FieldCartesianOrbit<>(toCartesian(date, current.propagateParameters(date)),
  336.                                          current.mean.getFrame(), mu);
  337.     }

  338.     /** Local class for Eckstein-Hechler model, with fixed mean parameters. */
  339.     private static class FieldEHModel<T extends RealFieldElement<T>> {

  340.         /** Factory for the derivatives. */
  341.         private final FDSFactory<T> factory;

  342.         /** Mean FieldOrbit<T>. */
  343.         private final FieldCircularOrbit<T> mean;

  344.         /** Constant mass. */
  345.         private final T mass;
  346.         // CHECKSTYLE: stop JavadocVariable check

  347.         // preprocessed values
  348.         private final T xnotDot;
  349.         private final T rdpom;
  350.         private final T rdpomp;
  351.         private final T eps1;
  352.         private final T eps2;
  353.         private final T xim;
  354.         private final T ommD;
  355.         private final T rdl;
  356.         private final T aMD;

  357.         private final T kh;
  358.         private final T kl;

  359.         private final T ax1;
  360.         private final T ay1;
  361.         private final T as1;
  362.         private final T ac2;
  363.         private final T axy3;
  364.         private final T as3;
  365.         private final T ac4;
  366.         private final T as5;
  367.         private final T ac6;

  368.         private final T ex1;
  369.         private final T exx2;
  370.         private final T exy2;
  371.         private final T ex3;
  372.         private final T ex4;

  373.         private final T ey1;
  374.         private final T eyx2;
  375.         private final T eyy2;
  376.         private final T ey3;
  377.         private final T ey4;

  378.         private final T rx1;
  379.         private final T ry1;
  380.         private final T r2;
  381.         private final T r3;
  382.         private final T rl;

  383.         private final T iy1;
  384.         private final T ix1;
  385.         private final T i2;
  386.         private final T i3;
  387.         private final T ih;

  388.         private final T lx1;
  389.         private final T ly1;
  390.         private final T l2;
  391.         private final T l3;
  392.         private final T ll;

  393.         // CHECKSTYLE: resume JavadocVariable check

  394.         /** Create a model for specified mean FieldOrbit<T>.
  395.          * @param factory factory for the derivatives
  396.          * @param mean mean FieldOrbit<T>
  397.          * @param mass constant mass
  398.          * @param referenceRadius reference radius of the central body attraction model (m)
  399.          * @param mu central attraction coefficient (m³/s²)
  400.          * @param ck0 un-normalized zonal coefficients
  401.          */
  402.         FieldEHModel(final FDSFactory<T> factory, final FieldCircularOrbit<T> mean, final T mass,
  403.                      final double referenceRadius, final double mu, final double[] ck0) {

  404.             this.factory         = factory;
  405.             this.mean            = mean;
  406.             this.mass            = mass;
  407.             final T zero = mass.getField().getZero();
  408.             final T one  = mass.getField().getOne();
  409.             // preliminary processing
  410.             T q =  zero.add(referenceRadius).divide(mean.getA());
  411.             T ql = q.multiply(q);
  412.             final T g2 = ql.multiply(ck0[2]);
  413.             ql = ql.multiply(q);
  414.             final T g3 = ql.multiply(ck0[3]);
  415.             ql = ql.multiply(q);
  416.             final T g4 = ql.multiply(ck0[4]);
  417.             ql = ql.multiply(q);
  418.             final T g5 = ql.multiply(ck0[5]);
  419.             ql = ql.multiply(q);
  420.             final T g6 = ql.multiply(ck0[6]);

  421.             final T cosI1 = mean.getI().cos();
  422.             final T sinI1 = mean.getI().sin();
  423.             final T sinI2 = sinI1.multiply(sinI1);
  424.             final T sinI4 = sinI2.multiply(sinI2);
  425.             final T sinI6 = sinI2.multiply(sinI4);

  426.             if (sinI2.getReal() < 1.0e-10) {
  427.                 throw new OrekitException(OrekitMessages.ALMOST_EQUATORIAL_ORBIT,
  428.                                                FastMath.toDegrees(mean.getI().getReal()));
  429.             }

  430.             if (FastMath.abs(sinI2.getReal() - 4.0 / 5.0) < 1.0e-3) {
  431.                 throw new OrekitException(OrekitMessages.ALMOST_CRITICALLY_INCLINED_ORBIT,
  432.                                                FastMath.toDegrees(mean.getI().getReal()));
  433.             }

  434.             if (mean.getE().getReal() > 0.1) {
  435.                 // if 0.005 < e < 0.1 no error is triggered, but accuracy is poor
  436.                 throw new OrekitException(OrekitMessages.TOO_LARGE_ECCENTRICITY_FOR_PROPAGATION_MODEL,
  437.                                                mean.getE());
  438.             }

  439.             xnotDot = zero.add(mu).divide(mean.getA()).sqrt().divide(mean.getA());

  440.             rdpom = g2.multiply(-0.75).multiply(sinI2.multiply(-5.0).add(4.0));
  441.             rdpomp = g4.multiply(7.5).multiply(sinI2.multiply(-31.0 / 8.0).add(1.0).add( sinI4.multiply(49.0 / 16.0))).subtract(
  442.                     g6.multiply(13.125).multiply(one.subtract(sinI2.multiply(8.0)).add(sinI4.multiply(129.0 / 8.0)).subtract(sinI6.multiply(297.0 / 32.0)) ));


  443.             q = zero.add(3.0).divide(rdpom.multiply(32.0));
  444.             eps1 = q.multiply(g4).multiply(sinI2).multiply(sinI2.multiply(-35.0).add(30.0)).subtract(
  445.                    q.multiply(175.0).multiply(g6).multiply(sinI2).multiply(sinI2.multiply(-3.0).add(sinI4.multiply(2.0625)).add(1.0)));
  446.             q = sinI1.multiply(3.0).divide(rdpom.multiply(8.0));
  447.             eps2 = q.multiply(g3).multiply(sinI2.multiply(-5.0).add(4.0)).subtract(q.multiply(g5).multiply(sinI2.multiply(-35.0).add(sinI4.multiply(26.25)).add(10.0)));

  448.             xim = mean.getI();
  449.             ommD = cosI1.multiply(g2.multiply(1.50).subtract(g2.multiply(2.25).multiply(g2).multiply(sinI2.multiply(-19.0 / 6.0).add(2.5))).add(
  450.                             g4.multiply(0.9375).multiply(sinI2.multiply(7.0).subtract(4.0))).add(
  451.                             g6.multiply(3.28125).multiply(sinI2.multiply(-9.0).add(2.0).add(sinI4.multiply(8.25)))));

  452.             rdl = g2.multiply(-1.50).multiply(sinI2.multiply(-4.0).add(3.0)).add(1.0);
  453.             aMD = rdl.add(
  454.                     g2.multiply(2.25).multiply(g2.multiply(sinI2.multiply(-263.0 / 12.0 ).add(9.0).add(sinI4.multiply(341.0 / 24.0))))).add(
  455.                     g4.multiply(15.0 / 16.0).multiply(sinI2.multiply(-31.0).add(8.0).add(sinI4.multiply(24.5)))).add(
  456.                     g6.multiply(105.0 / 32.0).multiply(sinI2.multiply(25.0).add(-10.0 / 3.0).subtract(sinI4.multiply(48.75)).add(sinI6.multiply(27.5))));

  457.             final T qq   = g2.divide(rdl).multiply(-1.5);
  458.             final T qA   = g2.multiply(0.75).multiply(g2).multiply(sinI2);
  459.             final T qB   = g4.multiply(0.25).multiply(sinI2);
  460.             final T qC   = g6.multiply(105.0 / 16.0).multiply(sinI2);
  461.             final T qD   = g3.multiply(-0.75).multiply(sinI1);
  462.             final T qE   = g5.multiply(3.75).multiply(sinI1);
  463.             kh = zero.add(0.375).divide(rdpom);
  464.             kl = kh.divide(sinI1);

  465.             ax1 = qq.multiply(sinI2.multiply(-3.5).add(2.0));
  466.             ay1 = qq.multiply(sinI2.multiply(-2.5).add(2.0));
  467.             as1 = qD.multiply(sinI2.multiply(-5.0).add(4.0)).add(
  468.                   qE.multiply(sinI4.multiply(2.625).add(sinI2.multiply(-3.5)).add(1.0)));
  469.             ac2 = qq.multiply(sinI2).add(
  470.                   qA.multiply(7.0).multiply(sinI2.multiply(-3.0).add(2.0))).add(
  471.                   qB.multiply(sinI2.multiply(-17.5).add(15.0))).add(
  472.                   qC.multiply(sinI2.multiply(3.0).subtract(1.0).subtract(sinI4.multiply(33.0 / 16.0))));
  473.             axy3 = qq.multiply(3.5).multiply(sinI2);
  474.             as3 = qD.multiply(5.0 / 3.0).multiply(sinI2).add(
  475.                   qE.multiply(7.0 / 6.0).multiply(sinI2).multiply(sinI2.multiply(-1.125).add(1)));
  476.             ac4 = qA.multiply(sinI2).add(
  477.                   qB.multiply(4.375).multiply(sinI2)).add(
  478.                   qC.multiply(0.75).multiply(sinI4.multiply(1.1).subtract(sinI2)));

  479.             as5 = qE.multiply(21.0 / 80.0).multiply(sinI4);

  480.             ac6 = qC.multiply(-11.0 / 80.0).multiply(sinI4);

  481.             ex1 = qq.multiply(sinI2.multiply(-1.25).add(1.0));
  482.             exx2 = qq.multiply(0.5).multiply(sinI2.multiply(-5.0).add(3.0));
  483.             exy2 = qq.multiply(sinI2.multiply(-1.5).add(2.0));
  484.             ex3 = qq.multiply(7.0 / 12.0).multiply(sinI2);
  485.             ex4 = qq.multiply(17.0 / 8.0).multiply(sinI2);

  486.             ey1 = qq.multiply(sinI2.multiply(-1.75).add(1.0));
  487.             eyx2 = qq.multiply(sinI2.multiply(-3.0).add(1.0));
  488.             eyy2 = qq.multiply(sinI2.multiply(2.0).subtract(1.5));
  489.             ey3 = qq.multiply(7.0 / 12.0).multiply(sinI2);
  490.             ey4 = qq.multiply(17.0 / 8.0).multiply(sinI2);

  491.             q  = cosI1.multiply(qq).negate();
  492.             rx1 = q.multiply(3.5);
  493.             ry1 = q.multiply(-2.5);
  494.             r2 = q.multiply(-0.5);
  495.             r3 =  q.multiply(7.0 / 6.0);
  496.             rl = g3 .multiply( cosI1).multiply(sinI2.multiply(-15.0).add(4.0)).subtract(
  497.                  g5.multiply(2.5).multiply(cosI1).multiply(sinI2.multiply(-42.0).add(4.0).add(sinI4.multiply(52.5))));

  498.             q = qq.multiply(0.5).multiply(sinI1).multiply(cosI1);
  499.             iy1 =  q;
  500.             ix1 = q.negate();
  501.             i2 =  q;
  502.             i3 =  q.multiply(7.0 / 3.0);
  503.             ih = g3.negate().multiply(cosI1).multiply(sinI2.multiply(-5.0).add(4)).add(
  504.                  g5.multiply(2.5).multiply(cosI1).multiply(sinI2.multiply(-14.0).add(4.0).add(sinI4.multiply(10.5))));
  505.             lx1 = qq.multiply(sinI2.multiply(-77.0 / 8.0).add(7.0));
  506.             ly1 = qq.multiply(sinI2.multiply(55.0 / 8.0).subtract(7.50));
  507.             l2 = qq.multiply(sinI2.multiply(1.25).subtract(0.5));
  508.             l3 = qq.multiply(sinI2.multiply(77.0 / 24.0).subtract(7.0 / 6.0));
  509.             ll = g3.multiply(sinI2.multiply(53.0).subtract(4.0).add(sinI4.multiply(-57.5))).add(
  510.                  g5.multiply(2.5).multiply(sinI2.multiply(-96.0).add(4.0).add(sinI4.multiply(269.5).subtract(sinI6.multiply(183.75)))));

  511.         }

  512.         /** Extrapolate an FieldOrbit<T> up to a specific target date.
  513.          * @param date target date for the FieldOrbit<T>
  514.          * @return propagated parameters
  515.          */
  516.         public FieldDerivativeStructure<T>[] propagateParameters(final FieldAbsoluteDate<T> date) {
  517.             final Field<T> field = date.durationFrom(mean.getDate()).getField();
  518.             final T one = field.getOne();
  519.             final T zero = field.getZero();
  520.             // Keplerian evolution
  521.             final FieldDerivativeStructure<T> dt =
  522.                     factory.build(date.durationFrom(mean.getDate()), one, zero);
  523.             final FieldDerivativeStructure<T> xnot = dt.multiply(xnotDot);

  524.             // secular effects

  525.             // eccentricity
  526.             final FieldDerivativeStructure<T> x   = xnot.multiply(rdpom.add(rdpomp));
  527.             final FieldDerivativeStructure<T> cx  = x.cos();
  528.             final FieldDerivativeStructure<T> sx  = x.sin();
  529.             final FieldDerivativeStructure<T> exm = cx.multiply(mean.getCircularEx()).
  530.                                             add(sx.multiply(eps2.subtract(one.subtract(eps1).multiply(mean.getCircularEy()))));
  531.             final FieldDerivativeStructure<T> eym = sx.multiply(eps1.add(1.0).multiply(mean.getCircularEx())).
  532.                                             add(cx.multiply(mean.getCircularEy().subtract(eps2))).
  533.                                             add(eps2);
  534.             // no secular effect on inclination

  535.             // right ascension of ascending node
  536.             final FieldDerivativeStructure<T> omm =
  537.                             factory.build(normalizeAngle(mean.getRightAscensionOfAscendingNode().add(ommD.multiply(xnot.getValue())),
  538.                                                          zero.add(FastMath.PI)),
  539.                                           ommD.multiply(xnotDot),
  540.                                           zero);
  541.             // latitude argument
  542.             final FieldDerivativeStructure<T> xlm =
  543.                             factory.build(normalizeAngle(mean.getAlphaM().add(aMD.multiply(xnot.getValue())), zero.add(FastMath.PI)),
  544.                                           aMD.multiply(xnotDot),
  545.                                           zero);

  546.             // periodical terms
  547.             final FieldDerivativeStructure<T> cl1 = xlm.cos();
  548.             final FieldDerivativeStructure<T> sl1 = xlm.sin();
  549.             final FieldDerivativeStructure<T> cl2 = cl1.multiply(cl1).subtract(sl1.multiply(sl1));
  550.             final FieldDerivativeStructure<T> sl2 = cl1.multiply(sl1).add(sl1.multiply(cl1));
  551.             final FieldDerivativeStructure<T> cl3 = cl2.multiply(cl1).subtract(sl2.multiply(sl1));
  552.             final FieldDerivativeStructure<T> sl3 = cl2.multiply(sl1).add(sl2.multiply(cl1));
  553.             final FieldDerivativeStructure<T> cl4 = cl3.multiply(cl1).subtract(sl3.multiply(sl1));
  554.             final FieldDerivativeStructure<T> sl4 = cl3.multiply(sl1).add(sl3.multiply(cl1));
  555.             final FieldDerivativeStructure<T> cl5 = cl4.multiply(cl1).subtract(sl4.multiply(sl1));
  556.             final FieldDerivativeStructure<T> sl5 = cl4.multiply(sl1).add(sl4.multiply(cl1));
  557.             final FieldDerivativeStructure<T> cl6 = cl5.multiply(cl1).subtract(sl5.multiply(sl1));

  558.             final FieldDerivativeStructure<T> qh  = eym.subtract(eps2).multiply(kh);
  559.             final FieldDerivativeStructure<T> ql  = exm.multiply(kl);

  560.             final FieldDerivativeStructure<T> exmCl1 = exm.multiply(cl1);
  561.             final FieldDerivativeStructure<T> exmSl1 = exm.multiply(sl1);
  562.             final FieldDerivativeStructure<T> eymCl1 = eym.multiply(cl1);
  563.             final FieldDerivativeStructure<T> eymSl1 = eym.multiply(sl1);
  564.             final FieldDerivativeStructure<T> exmCl2 = exm.multiply(cl2);
  565.             final FieldDerivativeStructure<T> exmSl2 = exm.multiply(sl2);
  566.             final FieldDerivativeStructure<T> eymCl2 = eym.multiply(cl2);
  567.             final FieldDerivativeStructure<T> eymSl2 = eym.multiply(sl2);
  568.             final FieldDerivativeStructure<T> exmCl3 = exm.multiply(cl3);
  569.             final FieldDerivativeStructure<T> exmSl3 = exm.multiply(sl3);
  570.             final FieldDerivativeStructure<T> eymCl3 = eym.multiply(cl3);
  571.             final FieldDerivativeStructure<T> eymSl3 = eym.multiply(sl3);
  572.             final FieldDerivativeStructure<T> exmCl4 = exm.multiply(cl4);
  573.             final FieldDerivativeStructure<T> exmSl4 = exm.multiply(sl4);
  574.             final FieldDerivativeStructure<T> eymCl4 = eym.multiply(cl4);
  575.             final FieldDerivativeStructure<T> eymSl4 = eym.multiply(sl4);

  576.             // semi major axis
  577.             final FieldDerivativeStructure<T> rda = exmCl1.multiply(ax1).
  578.                                             add(eymSl1.multiply(ay1)).
  579.                                             add(sl1.multiply(as1)).
  580.                                             add(cl2.multiply(ac2)).
  581.                                             add(exmCl3.add(eymSl3).multiply(axy3)).
  582.                                             add(sl3.multiply(as3)).
  583.                                             add(cl4.multiply(ac4)).
  584.                                             add(sl5.multiply(as5)).
  585.                                             add(cl6.multiply(ac6));

  586.             // eccentricity
  587.             final FieldDerivativeStructure<T> rdex = cl1.multiply(ex1).
  588.                                              add(exmCl2.multiply(exx2)).
  589.                                              add(eymSl2.multiply(exy2)).
  590.                                              add(cl3.multiply(ex3)).
  591.                                              add(exmCl4.add(eymSl4).multiply(ex4));
  592.             final FieldDerivativeStructure<T> rdey = sl1.multiply(ey1).
  593.                                              add(exmSl2.multiply(eyx2)).
  594.                                              add(eymCl2.multiply(eyy2)).
  595.                                              add(sl3.multiply(ey3)).
  596.                                              add(exmSl4.subtract(eymCl4).multiply(ey4));

  597.             // ascending node
  598.             final FieldDerivativeStructure<T> rdom = exmSl1.multiply(rx1).
  599.                                              add(eymCl1.multiply(ry1)).
  600.                                              add(sl2.multiply(r2)).
  601.                                              add(eymCl3.subtract(exmSl3).multiply(r3)).
  602.                                              add(ql.multiply(rl));

  603.             // inclination
  604.             final FieldDerivativeStructure<T> rdxi = eymSl1.multiply(iy1).
  605.                                              add(exmCl1.multiply(ix1)).
  606.                                              add(cl2.multiply(i2)).
  607.                                              add(exmCl3.add(eymSl3).multiply(i3)).
  608.                                              add(qh.multiply(ih));

  609.             // latitude argument
  610.             final FieldDerivativeStructure<T> rdxl = exmSl1.multiply(lx1).
  611.                                              add(eymCl1.multiply(ly1)).
  612.                                              add(sl2.multiply(l2)).
  613.                                              add(exmSl3.subtract(eymCl3).multiply(l3)).
  614.                                              add(ql.multiply(ll));
  615.             // osculating parameters
  616.             final FieldDerivativeStructure<T>[] FTD = MathArrays.buildArray(rdxl.getField(), 6);

  617.             FTD[0] = rda.add(1.0).multiply(mean.getA());
  618.             FTD[1] = rdex.add(exm);
  619.             FTD[2] = rdey.add(eym);
  620.             FTD[3] = rdxi.add(xim);
  621.             FTD[4] = rdom.add(omm);
  622.             FTD[5] = rdxl.add(xlm);
  623.             return FTD;

  624.         }

  625.     }

  626.     /** Convert circular parameters <em>with derivatives</em> to Cartesian coordinates.
  627.      * @param date date of the FieldOrbit<T>al parameters
  628.      * @param parameters circular parameters (a, ex, ey, i, raan, alphaM)
  629.      * @return Cartesian coordinates consistent with values and derivatives
  630.      */
  631.     private TimeStampedFieldPVCoordinates<T> toCartesian(final FieldAbsoluteDate<T> date, final FieldDerivativeStructure<T>[] parameters) {

  632.         // evaluate coordinates in the FieldOrbit<T> canonical reference frame
  633.         final FieldDerivativeStructure<T> cosOmega = parameters[4].cos();
  634.         final FieldDerivativeStructure<T> sinOmega = parameters[4].sin();
  635.         final FieldDerivativeStructure<T> cosI     = parameters[3].cos();
  636.         final FieldDerivativeStructure<T> sinI     = parameters[3].sin();
  637.         final FieldDerivativeStructure<T> alphaE   = meanToEccentric(parameters[5], parameters[1], parameters[2]);
  638.         final FieldDerivativeStructure<T> cosAE    = alphaE.cos();
  639.         final FieldDerivativeStructure<T> sinAE    = alphaE.sin();
  640.         final FieldDerivativeStructure<T> ex2      = parameters[1].multiply(parameters[1]);
  641.         final FieldDerivativeStructure<T> ey2      = parameters[2].multiply(parameters[2]);
  642.         final FieldDerivativeStructure<T> exy      = parameters[1].multiply(parameters[2]);
  643.         final FieldDerivativeStructure<T> q        = ex2.add(ey2).subtract(1).negate().sqrt();
  644.         final FieldDerivativeStructure<T> beta     = q.add(1).reciprocal();
  645.         final FieldDerivativeStructure<T> bx2      = beta.multiply(ex2);
  646.         final FieldDerivativeStructure<T> by2      = beta.multiply(ey2);
  647.         final FieldDerivativeStructure<T> bxy      = beta.multiply(exy);
  648.         final FieldDerivativeStructure<T> u        = bxy.multiply(sinAE).subtract(parameters[1].add(by2.subtract(1).multiply(cosAE)));
  649.         final FieldDerivativeStructure<T> v        = bxy.multiply(cosAE).subtract(parameters[2].add(bx2.subtract(1).multiply(sinAE)));
  650.         final FieldDerivativeStructure<T> x        = parameters[0].multiply(u);
  651.         final FieldDerivativeStructure<T> y        = parameters[0].multiply(v);

  652.         // canonical FieldOrbit<T> reference frame
  653.         final FieldVector3D<FieldDerivativeStructure<T>> p =
  654.                 new FieldVector3D<FieldDerivativeStructure<T>>(x.multiply(cosOmega).subtract(y.multiply(cosI.multiply(sinOmega))),
  655.                                                        x.multiply(sinOmega).add(y.multiply(cosI.multiply(cosOmega))),
  656.                                                        y.multiply(sinI));

  657.         // dispatch derivatives
  658.         final FieldVector3D<T> p0 = new FieldVector3D<>(p.getX().getValue(),
  659.                                                         p.getY().getValue(),
  660.                                                         p.getZ().getValue());
  661.         final FieldVector3D<T> p1 = new FieldVector3D<>(p.getX().getPartialDerivative(1),
  662.                                                         p.getY().getPartialDerivative(1),
  663.                                                         p.getZ().getPartialDerivative(1));
  664.         final FieldVector3D<T> p2 = new FieldVector3D<>(p.getX().getPartialDerivative(2),
  665.                                                         p.getY().getPartialDerivative(2),
  666.                                                         p.getZ().getPartialDerivative(2));
  667.         return new TimeStampedFieldPVCoordinates<>(date, p0, p1, p2);

  668.     }

  669.     /** Computes the eccentric latitude argument from the mean latitude argument.
  670.      * @param alphaM = M + Ω mean latitude argument (rad)
  671.      * @param ex e cos(Ω), first component of circular eccentricity vector
  672.      * @param ey e sin(Ω), second component of circular eccentricity vector
  673.      * @return the eccentric latitude argument.
  674.      */
  675.     private FieldDerivativeStructure<T> meanToEccentric(final FieldDerivativeStructure<T> alphaM,
  676.                                                 final FieldDerivativeStructure<T> ex,
  677.                                                 final FieldDerivativeStructure<T> ey) {
  678.         // Generalization of Kepler equation to circular parameters
  679.         // with alphaE = PA + E and
  680.         //      alphaM = PA + M = alphaE - ex.sin(alphaE) + ey.cos(alphaE)
  681.         FieldDerivativeStructure<T> alphaE        = alphaM;
  682.         FieldDerivativeStructure<T> shift         = alphaM.getField().getZero();
  683.         FieldDerivativeStructure<T> alphaEMalphaM = alphaM.getField().getZero();
  684.         FieldDerivativeStructure<T> cosAlphaE     = alphaE.cos();
  685.         FieldDerivativeStructure<T> sinAlphaE     = alphaE.sin();
  686.         int                 iter          = 0;
  687.         do {
  688.             final FieldDerivativeStructure<T> f2 = ex.multiply(sinAlphaE).subtract(ey.multiply(cosAlphaE));
  689.             final FieldDerivativeStructure<T> f1 = alphaM.getField().getOne().subtract(ex.multiply(cosAlphaE)).subtract(ey.multiply(sinAlphaE));
  690.             final FieldDerivativeStructure<T> f0 = alphaEMalphaM.subtract(f2);

  691.             final FieldDerivativeStructure<T> f12 = f1.multiply(2);
  692.             shift = f0.multiply(f12).divide(f1.multiply(f12).subtract(f0.multiply(f2)));

  693.             alphaEMalphaM  = alphaEMalphaM.subtract(shift);
  694.             alphaE         = alphaM.add(alphaEMalphaM);
  695.             cosAlphaE      = alphaE.cos();
  696.             sinAlphaE      = alphaE.sin();

  697.         } while ((++iter < 50) && (FastMath.abs(shift.getValue().getReal()) > 1.0e-12));

  698.         return alphaE;

  699.     }

  700.     /** {@inheritDoc} */
  701.     protected T getMass(final FieldAbsoluteDate<T> date) {
  702.         return models.get(date).mass;
  703.     }
  704.     /**
  705.      * Normalize an angle in a 2&pi; wide interval around a center value.
  706.      * <p>This method has three main uses:</p>
  707.      * <ul>
  708.      *   <li>normalize an angle between 0 and 2&pi;:<br/>
  709.      *       {@code a = MathUtils.normalizeAngle(a, FastMath.PI);}</li>
  710.      *   <li>normalize an angle between -&pi; and +&pi;<br/>
  711.      *       {@code a = MathUtils.normalizeAngle(a, 0.0);}</li>
  712.      *   <li>compute the angle between two defining angular positions:<br>
  713.      *       {@code angle = MathUtils.normalizeAngle(end, start) - start;}</li>
  714.      * </ul>
  715.      * <p>Note that due to numerical accuracy and since &pi; cannot be represented
  716.      * exactly, the result interval is <em>closed</em>, it cannot be half-closed
  717.      * as would be more satisfactory in a purely mathematical view.</p>
  718.      * @param a angle to normalize
  719.      * @param center center of the desired 2&pi; interval for the result
  720.      * @param <T> the type of the field elements
  721.      * @return a-2k&pi; with integer k and center-&pi; &lt;= a-2k&pi; &lt;= center+&pi;
  722.      * @since 1.2
  723.      */
  724.     public static <T extends RealFieldElement<T>> T normalizeAngle(final T a, final T center) {
  725.         return a.subtract(2 * FastMath.PI * FastMath.floor((a.getReal() + FastMath.PI - center.getReal()) / (2 * FastMath.PI)));
  726.     }


  727. }