FieldBrouwerLyddanePropagator.java

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

  19. import java.util.Collections;
  20. import java.util.List;

  22. import org.hipparchus.CalculusFieldElement;
  23. import org.hipparchus.Field;
  24. import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2;
  25. import org.hipparchus.util.CombinatoricsUtils;
  26. import org.hipparchus.util.FastMath;
  27. import org.hipparchus.util.FieldSinCos;
  28. import org.hipparchus.util.MathArrays;
  29. import org.hipparchus.util.MathUtils;
  30. import org.hipparchus.util.Precision;
  31. import org.orekit.attitudes.AttitudeProvider;
  32. import org.orekit.attitudes.InertialProvider;
  33. import org.orekit.errors.OrekitException;
  34. import org.orekit.errors.OrekitMessages;
  35. import org.orekit.forces.gravity.potential.UnnormalizedSphericalHarmonicsProvider;
  36. import org.orekit.forces.gravity.potential.UnnormalizedSphericalHarmonicsProvider.UnnormalizedSphericalHarmonics;
  37. import org.orekit.orbits.FieldKeplerianOrbit;
  38. import org.orekit.orbits.FieldOrbit;
  39. import org.orekit.orbits.OrbitType;
  40. import org.orekit.orbits.PositionAngle;
  41. import org.orekit.propagation.FieldSpacecraftState;
  42. import org.orekit.propagation.PropagationType;
  43. import org.orekit.propagation.analytical.tle.FieldTLE;
  44. import org.orekit.time.FieldAbsoluteDate;
  45. import org.orekit.utils.FieldTimeSpanMap;
  46. import org.orekit.utils.ParameterDriver;

  48. /** This class propagates a {@link org.orekit.propagation.FieldSpacecraftState}
  49.  *  using the analytical Brouwer-Lyddane model (from J2 to J5 zonal harmonics).
  50.  * <p>
  51.  * At the opposite of the {@link FieldEcksteinHechlerPropagator}, the Brouwer-Lyddane model is
  52.  * suited for elliptical orbits, there is no problem having a rather small eccentricity or inclination
  53.  * (Lyddane helped to solve this issue with the Brouwer model). Singularity for the critical
  54.  * inclination i = 63.4° is avoided using the method developed in Warren Phipps' 1992 thesis.
  55.  * <p>
  56.  * By default, Brouwer-Lyddane model considers only the perturbations due to zonal harmonics.
  57.  * However, for low Earth orbits, the magnitude of the perturbative acceleration due to
  58.  * atmospheric drag can be significant. Warren Phipps' 1992 thesis considered the atmospheric
  59.  * drag by time derivatives of the <i>mean</i> mean anomaly using the catch-all coefficient
  60.  * {@link #M2Driver}.
  61.  *
  62.  * Usually, M2 is adjusted during an orbit determination process and it represents the
  63.  * combination of all unmodeled secular along-track effects (i.e. not just the atmospheric drag).
  64.  * The behavior of M2 is close to the {@link FieldTLE#getBStar()} parameter for the TLE.
  65.  *
  66.  * If the value of M2 is equal to {@link BrouwerLyddanePropagator#M2 0.0}, the along-track secular
  67.  * effects are not considered in the dynamical model. Typical values for M2 are not known.
  68.  * It depends on the orbit type. However, the value of M2 must be very small (e.g. between 1.0e-14 and 1.0e-15).
  69.  * The unit of M2 is rad/s².
  70.  *
  71.  * The along-track effects, represented by the secular rates of the mean semi-major axis
  72.  * and eccentricity, are computed following Eq. 2.38, 2.41, and 2.45 of Warren Phipps' thesis.
  73.  *
  74.  * @see "Brouwer, Dirk. Solution of the problem of artificial satellite theory without drag.
  75.  *       YALE UNIV NEW HAVEN CT NEW HAVEN United States, 1959."
  76.  *
  77.  * @see "Lyddane, R. H. Small eccentricities or inclinations in the Brouwer theory of the
  78.  *       artificial satellite. The Astronomical Journal 68 (1963): 555."
  79.  *
  80.  * @see "Phipps Jr, Warren E. Parallelization of the Navy Space Surveillance Center
  81.  *       (NAVSPASUR) Satellite Model. NAVAL POSTGRADUATE SCHOOL MONTEREY CA, 1992."
  82.  *
  83.  * @author Melina Vanel
  84.  * @author Bryan Cazabonne
  85.  * @since 11.1
  86.  */
  87. public class FieldBrouwerLyddanePropagator<T extends CalculusFieldElement<T>> extends FieldAbstractAnalyticalPropagator<T>  {

  89.     /** Parameters scaling factor.
  90.      * <p>
  91.      * We use a power of 2 to avoid numeric noise introduction
  92.      * in the multiplications/divisions sequences.
  93.      * </p>
  94.      */
  95.     private static final double SCALE = FastMath.scalb(1.0, -20);

  97.     /** Beta constant used by T2 function. */
  98.     private static final double BETA = FastMath.scalb(100, -11);

  100.     /** Initial Brouwer-Lyddane model. */
  101.     private FieldBLModel<T> initialModel;

  103.     /** All models. */
  104.     private transient FieldTimeSpanMap<FieldBLModel<T>, T> models;

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

  109.     /** Central attraction coefficient (m³/s²). */
  110.     private T mu;

  112.     /** Un-normalized zonal coefficients. */
  113.     private double[] ck0;

  115.     /** Empirical coefficient used in the drag modeling. */
  116.     private final ParameterDriver M2Driver;

  118.     /** Build a propagator from orbit and potential provider.
  119.      * <p>Mass and attitude provider are set to unspecified non-null arbitrary values.</p>
  120.      *
  121.      * <p>Using this constructor, an initial osculating orbit is considered.</p>
  122.      *
  123.      * @param initialOrbit initial orbit
  124.      * @param provider for un-normalized zonal coefficients
  125.      * @param M2 value of empirical drag coefficient in rad/s².
  126.      *        If equal to {@link BrouwerLyddanePropagator#M2} drag is not computed
  127.      * @see #FieldBrouwerLyddanePropagator(FieldOrbit, UnnormalizedSphericalHarmonicsProvider, PropagationType, double)
  128.      */
  129.     public FieldBrouwerLyddanePropagator(final FieldOrbit<T> initialOrbit,
  130.                                          final UnnormalizedSphericalHarmonicsProvider provider,
  131.                                          final double M2) {
  132.         this(initialOrbit, InertialProvider.of(initialOrbit.getFrame()),
  133.              initialOrbit.getA().getField().getZero().add(DEFAULT_MASS), provider,
  134.              provider.onDate(initialOrbit.getDate().toAbsoluteDate()), M2);
  135.     }

  137.     /**
  138.      * Private helper constructor.
  139.      * <p>Using this constructor, an initial osculating orbit is considered.</p>
  140.      * @param initialOrbit initial orbit
  141.      * @param attitude attitude provider
  142.      * @param mass spacecraft mass
  143.      * @param provider for un-normalized zonal coefficients
  144.      * @param harmonics {@code provider.onDate(initialOrbit.getDate())}
  145.      * @param M2 value of empirical drag coefficient in rad/s².
  146.      *        If equal to {@link BrouwerLyddanePropagator#M2} drag is not computed
  147.      * @see #FieldBrouwerLyddanePropagator(FieldOrbit, AttitudeProvider, CalculusFieldElement,
  148.      * UnnormalizedSphericalHarmonicsProvider, UnnormalizedSphericalHarmonicsProvider.UnnormalizedSphericalHarmonics, PropagationType, double)
  149.      */
  150.     public FieldBrouwerLyddanePropagator(final FieldOrbit<T> initialOrbit,
  151.                                          final AttitudeProvider attitude,
  152.                                          final T mass,
  153.                                          final UnnormalizedSphericalHarmonicsProvider provider,
  154.                                          final UnnormalizedSphericalHarmonics harmonics,
  155.                                          final double M2) {
  156.         this(initialOrbit, attitude,  mass, provider.getAe(), initialOrbit.getA().getField().getZero().add(provider.getMu()),
  157.              harmonics.getUnnormalizedCnm(2, 0),
  158.              harmonics.getUnnormalizedCnm(3, 0),
  159.              harmonics.getUnnormalizedCnm(4, 0),
  160.              harmonics.getUnnormalizedCnm(5, 0),
  161.              M2);
  162.     }

  164.     /** Build a propagator from orbit and potential.
  165.      * <p>Mass and attitude provider are set to unspecified non-null arbitrary values.</p>
  166.      * <p>The C<sub>n,0</sub> coefficients are the denormalized zonal coefficients, they
  167.      * are related to both the normalized coefficients
  168.      * <span style="text-decoration: overline">C</span><sub>n,0</sub>
  169.      *  and the J<sub>n</sub> one as follows:</p>
  170.      *
  171.      * <p> C<sub>n,0</sub> = [(2-δ<sub>0,m</sub>)(2n+1)(n-m)!/(n+m)!]<sup>½</sup>
  172.      * <span style="text-decoration: overline">C</span><sub>n,0</sub>
  173.      *
  174.      * <p> C<sub>n,0</sub> = -J<sub>n</sub>
  175.      *
  176.      * <p>Using this constructor, an initial osculating orbit is considered.</p>
  177.      *
  178.      * @param initialOrbit initial orbit
  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 M2 value of empirical drag coefficient in rad/s².
  186.      *        If equal to {@link BrouwerLyddanePropagator#M2} drag is not computed
  187.      * @see org.orekit.utils.Constants
  188.      * @see #FieldBrouwerLyddanePropagator(FieldOrbit, AttitudeProvider, double, CalculusFieldElement, double, double, double, double, double)
  189.      */
  190.     public FieldBrouwerLyddanePropagator(final FieldOrbit<T> initialOrbit,
  191.                                          final double referenceRadius, final T mu,
  192.                                          final double c20, final double c30, final double c40,
  193.                                          final double c50, final double M2) {
  194.         this(initialOrbit, InertialProvider.of(initialOrbit.getFrame()),
  195.              initialOrbit.getDate().getField().getZero().add(DEFAULT_MASS),
  196.              referenceRadius, mu, c20, c30, c40, c50, M2);
  197.     }

  199.     /** Build a propagator from orbit, mass and potential provider.
  200.      * <p>Attitude law is set to an unspecified non-null arbitrary value.</p>
  201.      *
  202.      * <p>Using this constructor, an initial osculating orbit is considered.</p>
  203.      *
  204.      * @param initialOrbit initial orbit
  205.      * @param mass spacecraft mass
  206.      * @param provider for un-normalized zonal coefficients
  207.      * @param M2 value of empirical drag coefficient in rad/s².
  208.      *        If equal to {@link BrouwerLyddanePropagator#M2} drag is not computed
  209.      * @see #FieldBrouwerLyddanePropagator(FieldOrbit, AttitudeProvider, CalculusFieldElement, UnnormalizedSphericalHarmonicsProvider, double)
  210.      */
  211.     public FieldBrouwerLyddanePropagator(final FieldOrbit<T> initialOrbit, final T mass,
  212.                                          final UnnormalizedSphericalHarmonicsProvider provider,
  213.                                          final double M2) {
  214.         this(initialOrbit, InertialProvider.of(initialOrbit.getFrame()),
  215.              mass, provider, provider.onDate(initialOrbit.getDate().toAbsoluteDate()), M2);
  216.     }

  218.     /** Build a propagator from orbit, mass and potential.
  219.      * <p>Attitude law is set to an unspecified non-null arbitrary value.</p>
  220.      * <p>The C<sub>n,0</sub> coefficients are the denormalized zonal coefficients, they
  221.      * are related to both the normalized coefficients
  222.      * <span style="text-decoration: overline">C</span><sub>n,0</sub>
  223.      *  and the J<sub>n</sub> one as follows:</p>
  224.      *
  225.      * <p> C<sub>n,0</sub> = [(2-δ<sub>0,m</sub>)(2n+1)(n-m)!/(n+m)!]<sup>½</sup>
  226.      * <span style="text-decoration: overline">C</span><sub>n,0</sub>
  227.      *
  228.      * <p> C<sub>n,0</sub> = -J<sub>n</sub>
  229.      *
  230.      * <p>Using this constructor, an initial osculating orbit is considered.</p>
  231.      *
  232.      * @param initialOrbit initial orbit
  233.      * @param mass spacecraft mass
  234.      * @param referenceRadius reference radius of the Earth for the potential model (m)
  235.      * @param mu central attraction coefficient (m³/s²)
  236.      * @param c20 un-normalized zonal coefficient (about -1.08e-3 for Earth)
  237.      * @param c30 un-normalized zonal coefficient (about +2.53e-6 for Earth)
  238.      * @param c40 un-normalized zonal coefficient (about +1.62e-6 for Earth)
  239.      * @param c50 un-normalized zonal coefficient (about +2.28e-7 for Earth)
  240.      * @param M2 value of empirical drag coefficient in rad/s².
  241.      *        If equal to {@link BrouwerLyddanePropagator#M2} drag is not computed
  242.      * @see #FieldBrouwerLyddanePropagator(FieldOrbit, AttitudeProvider, CalculusFieldElement, double, CalculusFieldElement, double, double, double, double, double)
  243.      */
  244.     public FieldBrouwerLyddanePropagator(final FieldOrbit<T> initialOrbit, final T mass,
  245.                                          final double referenceRadius, final T mu,
  246.                                          final double c20, final double c30, final double c40,
  247.                                          final double c50, final double M2) {
  248.         this(initialOrbit, InertialProvider.of(initialOrbit.getFrame()),
  249.              mass, referenceRadius, mu, c20, c30, c40, c50, M2);
  250.     }

  252.     /** Build a propagator from orbit, attitude provider and potential provider.
  253.      * <p>Mass is set to an unspecified non-null arbitrary value.</p>
  254.      * <p>Using this constructor, an initial osculating orbit is considered.</p>
  255.      * @param initialOrbit initial orbit
  256.      * @param attitudeProv attitude provider
  257.      * @param provider for un-normalized zonal coefficients
  258.      * @param M2 value of empirical drag coefficient in rad/s².
  259.      *        If equal to {@link BrouwerLyddanePropagator#M2} drag is not computed
  260.      */
  261.     public FieldBrouwerLyddanePropagator(final FieldOrbit<T> initialOrbit,
  262.                                          final AttitudeProvider attitudeProv,
  263.                                          final UnnormalizedSphericalHarmonicsProvider provider,
  264.                                          final double M2) {
  265.         this(initialOrbit, attitudeProv, initialOrbit.getA().getField().getZero().add(DEFAULT_MASS), provider,
  266.              provider.onDate(initialOrbit.getDate().toAbsoluteDate()), M2);
  267.     }

  269.     /** Build a propagator from orbit, attitude provider and potential.
  270.      * <p>Mass is set to an unspecified non-null arbitrary value.</p>
  271.      * <p>The C<sub>n,0</sub> coefficients are the denormalized zonal coefficients, they
  272.      * are related to both the normalized coefficients
  273.      * <span style="text-decoration: overline">C</span><sub>n,0</sub>
  274.      *  and the J<sub>n</sub> one as follows:</p>
  275.      *
  276.      * <p> C<sub>n,0</sub> = [(2-δ<sub>0,m</sub>)(2n+1)(n-m)!/(n+m)!]<sup>½</sup>
  277.      * <span style="text-decoration: overline">C</span><sub>n,0</sub>
  278.      *
  279.      * <p> C<sub>n,0</sub> = -J<sub>n</sub>
  280.      *
  281.      * <p>Using this constructor, an initial osculating orbit is considered.</p>
  282.      *
  283.      * @param initialOrbit initial orbit
  284.      * @param attitudeProv attitude provider
  285.      * @param referenceRadius reference radius of the Earth for the potential model (m)
  286.      * @param mu central attraction coefficient (m³/s²)
  287.      * @param c20 un-normalized zonal coefficient (about -1.08e-3 for Earth)
  288.      * @param c30 un-normalized zonal coefficient (about +2.53e-6 for Earth)
  289.      * @param c40 un-normalized zonal coefficient (about +1.62e-6 for Earth)
  290.      * @param c50 un-normalized zonal coefficient (about +2.28e-7 for Earth)
  291.      * @param M2 value of empirical drag coefficient in rad/s².
  292.      *        If equal to {@link BrouwerLyddanePropagator#M2} drag is not computed
  293.      */
  294.     public FieldBrouwerLyddanePropagator(final FieldOrbit<T> initialOrbit,
  295.                                          final AttitudeProvider attitudeProv,
  296.                                          final double referenceRadius, final T mu,
  297.                                          final double c20, final double c30, final double c40,
  298.                                          final double c50, final double M2) {
  299.         this(initialOrbit, attitudeProv, initialOrbit.getDate().getField().getZero().add(DEFAULT_MASS),
  300.              referenceRadius, mu, c20, c30, c40, c50, M2);
  301.     }

  303.     /** Build a propagator from orbit, attitude provider, mass and potential provider.
  304.      * <p>Using this constructor, an initial osculating orbit is considered.</p>
  305.      * @param initialOrbit initial orbit
  306.      * @param attitudeProv attitude provider
  307.      * @param mass spacecraft mass
  308.      * @param provider for un-normalized zonal coefficients
  309.      * @param M2 value of empirical drag coefficient in rad/s².
  310.      *        If equal to {@link BrouwerLyddanePropagator#M2} drag is not computed
  311.      * @see #FieldBrouwerLyddanePropagator(FieldOrbit, AttitudeProvider, CalculusFieldElement, UnnormalizedSphericalHarmonicsProvider, PropagationType, double)
  312.      */
  313.     public FieldBrouwerLyddanePropagator(final FieldOrbit<T> initialOrbit,
  314.                                          final AttitudeProvider attitudeProv,
  315.                                          final T mass,
  316.                                          final UnnormalizedSphericalHarmonicsProvider provider,
  317.                                          final double M2) {
  318.         this(initialOrbit, attitudeProv, mass, provider, provider.onDate(initialOrbit.getDate().toAbsoluteDate()), M2);
  319.     }

  321.     /** Build a propagator from orbit, attitude provider, mass and potential.
  322.      * <p>The C<sub>n,0</sub> coefficients are the denormalized zonal coefficients, they
  323.      * are related to both the normalized coefficients
  324.      * <span style="text-decoration: overline">C</span><sub>n,0</sub>
  325.      *  and the J<sub>n</sub> one as follows:</p>
  326.      *
  327.      * <p> C<sub>n,0</sub> = [(2-δ<sub>0,m</sub>)(2n+1)(n-m)!/(n+m)!]<sup>½</sup>
  328.      * <span style="text-decoration: overline">C</span><sub>n,0</sub>
  329.      *
  330.      * <p> C<sub>n,0</sub> = -J<sub>n</sub>
  331.      *
  332.      * <p>Using this constructor, an initial osculating orbit is considered.</p>
  333.      *
  334.      * @param initialOrbit initial orbit
  335.      * @param attitudeProv attitude provider
  336.      * @param mass spacecraft mass
  337.      * @param referenceRadius reference radius of the Earth for the potential model (m)
  338.      * @param mu central attraction coefficient (m³/s²)
  339.      * @param c20 un-normalized zonal coefficient (about -1.08e-3 for Earth)
  340.      * @param c30 un-normalized zonal coefficient (about +2.53e-6 for Earth)
  341.      * @param c40 un-normalized zonal coefficient (about +1.62e-6 for Earth)
  342.      * @param c50 un-normalized zonal coefficient (about +2.28e-7 for Earth)
  343.      * @param M2 value of empirical drag coefficient in rad/s².
  344.      *        If equal to {@link BrouwerLyddanePropagator#M2} drag is not computed
  345.      * @see #FieldBrouwerLyddanePropagator(FieldOrbit, AttitudeProvider, CalculusFieldElement, double, CalculusFieldElement, double, double, double, double, PropagationType, double)
  346.      */
  347.     public FieldBrouwerLyddanePropagator(final FieldOrbit<T> initialOrbit,
  348.                                          final AttitudeProvider attitudeProv,
  349.                                          final T mass,
  350.                                          final double referenceRadius, final T mu,
  351.                                          final double c20, final double c30, final double c40,
  352.                                          final double c50, final double M2) {
  353.         this(initialOrbit, attitudeProv, mass, referenceRadius, mu, c20, c30, c40, c50, PropagationType.OSCULATING, M2);
  354.     }


  357.     /** Build a propagator from orbit and potential provider.
  358.      * <p>Mass and attitude provider are set to unspecified non-null arbitrary values.</p>
  359.      *
  360.      * <p>Using this constructor, it is possible to define the initial orbit as
  361.      * a mean Brouwer-Lyddane orbit or an osculating one.</p>
  362.      *
  363.      * @param initialOrbit initial orbit
  364.      * @param provider for un-normalized zonal coefficients
  365.      * @param initialType initial orbit type (mean Brouwer-Lyddane orbit or osculating orbit)
  366.      * @param M2 value of empirical drag coefficient in rad/s².
  367.      *        If equal to {@link BrouwerLyddanePropagator#M2} drag is not computed
  368.      */
  369.     public FieldBrouwerLyddanePropagator(final FieldOrbit<T> initialOrbit,
  370.                                          final UnnormalizedSphericalHarmonicsProvider provider,
  371.                                          final PropagationType initialType,
  372.                                          final double M2) {
  373.         this(initialOrbit, InertialProvider.of(initialOrbit.getFrame()),
  374.              initialOrbit.getA().getField().getZero().add(DEFAULT_MASS), provider,
  375.              provider.onDate(initialOrbit.getDate().toAbsoluteDate()), initialType, M2);
  376.     }

  378.     /** Build a propagator from orbit, attitude provider, mass and potential provider.
  379.      * <p>Using this constructor, it is possible to define the initial orbit as
  380.      * a mean Brouwer-Lyddane orbit or an osculating one.</p>
  381.      * @param initialOrbit initial orbit
  382.      * @param attitudeProv attitude provider
  383.      * @param mass spacecraft mass
  384.      * @param provider for un-normalized zonal coefficients
  385.      * @param initialType initial orbit type (mean Brouwer-Lyddane orbit or osculating orbit)
  386.      * @param M2 value of empirical drag coefficient in rad/s².
  387.      *        If equal to {@link BrouwerLyddanePropagator#M2} drag is not computed
  388.      */
  389.     public FieldBrouwerLyddanePropagator(final FieldOrbit<T> initialOrbit,
  390.                                          final AttitudeProvider attitudeProv,
  391.                                          final T mass,
  392.                                          final UnnormalizedSphericalHarmonicsProvider provider,
  393.                                          final PropagationType initialType,
  394.                                          final double M2) {
  395.         this(initialOrbit, attitudeProv, mass, provider, provider.onDate(initialOrbit.getDate().toAbsoluteDate()), initialType, M2);
  396.     }

  398.     /**
  399.      * Private helper constructor.
  400.      * <p>Using this constructor, it is possible to define the initial orbit as
  401.      * a mean Brouwer-Lyddane orbit or an osculating one.</p>
  402.      * @param initialOrbit initial orbit
  403.      * @param attitude attitude provider
  404.      * @param mass spacecraft mass
  405.      * @param provider for un-normalized zonal coefficients
  406.      * @param harmonics {@code provider.onDate(initialOrbit.getDate())}
  407.      * @param initialType initial orbit type (mean Brouwer-Lyddane orbit or osculating orbit)
  408.      * @param M2 value of empirical drag coefficient in rad/s².
  409.      *        If equal to {@link BrouwerLyddanePropagator#M2} drag is not computed
  410.      */
  411.     public FieldBrouwerLyddanePropagator(final FieldOrbit<T> initialOrbit,
  412.                                          final AttitudeProvider attitude,
  413.                                          final T mass,
  414.                                          final UnnormalizedSphericalHarmonicsProvider provider,
  415.                                          final UnnormalizedSphericalHarmonics harmonics,
  416.                                          final PropagationType initialType,
  417.                                          final double M2) {
  418.         this(initialOrbit, attitude, mass, provider.getAe(), initialOrbit.getA().getField().getZero().add(provider.getMu()),
  419.              harmonics.getUnnormalizedCnm(2, 0),
  420.              harmonics.getUnnormalizedCnm(3, 0),
  421.              harmonics.getUnnormalizedCnm(4, 0),
  422.              harmonics.getUnnormalizedCnm(5, 0),
  423.              initialType, M2);
  424.     }

  426.     /** Build a propagator from orbit, attitude provider, mass and potential.
  427.      * <p>The C<sub>n,0</sub> coefficients are the denormalized zonal coefficients, they
  428.      * are related to both the normalized coefficients
  429.      * <span style="text-decoration: overline">C</span><sub>n,0</sub>
  430.      *  and the J<sub>n</sub> one as follows:</p>
  431.      *
  432.      * <p> C<sub>n,0</sub> = [(2-δ<sub>0,m</sub>)(2n+1)(n-m)!/(n+m)!]<sup>½</sup>
  433.      * <span style="text-decoration: overline">C</span><sub>n,0</sub>
  434.      *
  435.      * <p> C<sub>n,0</sub> = -J<sub>n</sub>
  436.      *
  437.      * <p>Using this constructor, it is possible to define the initial orbit as
  438.      * a mean Brouwer-Lyddane orbit or an osculating one.</p>
  439.      *
  440.      * @param initialOrbit initial orbit
  441.      * @param attitudeProv attitude provider
  442.      * @param mass spacecraft mass
  443.      * @param referenceRadius reference radius of the Earth for the potential model (m)
  444.      * @param mu central attraction coefficient (m³/s²)
  445.      * @param c20 un-normalized zonal coefficient (about -1.08e-3 for Earth)
  446.      * @param c30 un-normalized zonal coefficient (about +2.53e-6 for Earth)
  447.      * @param c40 un-normalized zonal coefficient (about +1.62e-6 for Earth)
  448.      * @param c50 un-normalized zonal coefficient (about +2.28e-7 for Earth)
  449.      * @param initialType initial orbit type (mean Brouwer-Lyddane orbit or osculating orbit)
  450.      * @param M2 value of empirical drag coefficient in rad/s².
  451.      *        If equal to {@link BrouwerLyddanePropagator#M2} drag is not computed
  452.      */
  453.     public FieldBrouwerLyddanePropagator(final FieldOrbit<T> initialOrbit,
  454.                                          final AttitudeProvider attitudeProv,
  455.                                          final T mass,
  456.                                          final double referenceRadius, final T mu,
  457.                                          final double c20, final double c30, final double c40,
  458.                                          final double c50,
  459.                                          final PropagationType initialType,
  460.                                          final double M2) {

  462.         super(mass.getField(), attitudeProv);

  464.         // store model coefficients
  465.         this.referenceRadius = referenceRadius;
  466.         this.mu  = mu;
  467.         this.ck0 = new double[] {0.0, 0.0, c20, c30, c40, c50};

  469.         // initialize M2 driver
  470.         this.M2Driver = new ParameterDriver(BrouwerLyddanePropagator.M2_NAME, M2, SCALE,
  471.                                             Double.NEGATIVE_INFINITY,
  472.                                             Double.POSITIVE_INFINITY);

  474.         // compute mean parameters if needed
  475.         resetInitialState(new FieldSpacecraftState<>(initialOrbit,
  476.                                                  attitudeProv.getAttitude(initialOrbit,
  477.                                                                           initialOrbit.getDate(),
  478.                                                                           initialOrbit.getFrame()),
  479.                                                  mass),
  480.                                                  initialType);

  482.     }


  485.     /** {@inheritDoc}
  486.      * <p>The new initial state to consider
  487.      * must be defined with an osculating orbit.</p>
  488.      * @see #resetInitialState(FieldSpacecraftState, PropagationType)
  489.      */
  490.     @Override
  491.     public void resetInitialState(final FieldSpacecraftState<T> state) {
  492.         resetInitialState(state, PropagationType.OSCULATING);
  493.     }

  495.     /** Reset the propagator initial state.
  496.      * @param state new initial state to consider
  497.      * @param stateType mean Brouwer-Lyddane orbit or osculating orbit
  498.      * @since 10.2
  499.      */
  500.     public void resetInitialState(final FieldSpacecraftState<T> state, final PropagationType stateType) {
  501.         super.resetInitialState(state);
  502.         final FieldKeplerianOrbit<T> keplerian = (FieldKeplerianOrbit<T>) OrbitType.KEPLERIAN.convertType(state.getOrbit());
  503.         this.initialModel = (stateType == PropagationType.MEAN) ?
  504.                              new FieldBLModel<>(keplerian, state.getMass(), referenceRadius, mu, ck0) :
  505.                              computeMeanParameters(keplerian, state.getMass());
  506.         this.models = new FieldTimeSpanMap<FieldBLModel<T>, T>(initialModel, state.getA().getField());
  507.     }

  509.     /** {@inheritDoc} */
  510.     @Override
  511.     protected void resetIntermediateState(final FieldSpacecraftState<T> state, final boolean forward) {
  512.         final FieldBLModel<T> newModel = computeMeanParameters((FieldKeplerianOrbit<T>) OrbitType.KEPLERIAN.convertType(state.getOrbit()),
  513.                                                        state.getMass());
  514.         if (forward) {
  515.             models.addValidAfter(newModel, state.getDate());
  516.         } else {
  517.             models.addValidBefore(newModel, state.getDate());
  518.         }
  519.         stateChanged(state);
  520.     }

  522.     /** Compute mean parameters according to the Brouwer-Lyddane analytical model computation
  523.      * in order to do the propagation.
  524.      * @param osculating osculating orbit
  525.      * @param mass constant mass
  526.      * @return Brouwer-Lyddane mean model
  527.      */
  528.     private FieldBLModel<T> computeMeanParameters(final FieldKeplerianOrbit<T> osculating, final T mass) {

  530.         // sanity check
  531.         if (osculating.getA().getReal() < referenceRadius) {
  532.             throw new OrekitException(OrekitMessages.TRAJECTORY_INSIDE_BRILLOUIN_SPHERE,
  533.                                            osculating.getA());
  534.         }

  536.         final Field<T> field = mass.getField();
  537.         final T one = field.getOne();
  538.         final T zero = field.getZero();
  539.         // rough initialization of the mean parameters
  540.         FieldBLModel<T> current = new FieldBLModel<T>(osculating, mass, referenceRadius, mu, ck0);

  542.         // threshold for each parameter
  543.         final T epsilon         = one.multiply(1.0e-13);
  544.         final T thresholdA      = epsilon.multiply(current.mean.getA().abs().add(1.0));
  545.         final T thresholdE      = epsilon.multiply(current.mean.getE().add(1.0));
  546.         final T thresholdAngles = epsilon.multiply(one.getPi());

  548.         int i = 0;
  549.         while (i++ < 200) {

  551.             // recompute the osculating parameters from the current mean parameters
  552.             final FieldUnivariateDerivative2<T>[] parameters = current.propagateParameters(current.mean.getDate(), getParameters(mass.getField()));

  554.             // adapted parameters residuals
  555.             final T deltaA     = osculating.getA()  .subtract(parameters[0].getValue());
  556.             final T deltaE     = osculating.getE()  .subtract(parameters[1].getValue());
  557.             final T deltaI     = osculating.getI()  .subtract(parameters[2].getValue());
  558.             final T deltaOmega = MathUtils.normalizeAngle(osculating.getPerigeeArgument().subtract(parameters[3].getValue()), zero);
  559.             final T deltaRAAN  = MathUtils.normalizeAngle(osculating.getRightAscensionOfAscendingNode().subtract(parameters[4].getValue()), zero);
  560.             final T deltaAnom  = MathUtils.normalizeAngle(osculating.getMeanAnomaly().subtract(parameters[5].getValue()), zero);


  563.             // update mean parameters
  564.             current = new FieldBLModel<T>(new FieldKeplerianOrbit<T>(current.mean.getA()            .add(deltaA),
  565.                                                      current.mean.getE()                            .add(deltaE),
  566.                                                      current.mean.getI()                            .add(deltaI),
  567.                                                      current.mean.getPerigeeArgument()              .add(deltaOmega),
  568.                                                      current.mean.getRightAscensionOfAscendingNode().add(deltaRAAN),
  569.                                                      current.mean.getMeanAnomaly()                  .add(deltaAnom),
  570.                                                      PositionAngle.MEAN,
  571.                                                      current.mean.getFrame(),
  572.                                                      current.mean.getDate(), mu),
  573.                                   mass, referenceRadius, mu, ck0);
  574.             // check convergence
  575.             if (FastMath.abs(deltaA.getReal())     < thresholdA.getReal() &&
  576.                 FastMath.abs(deltaE.getReal())     < thresholdE.getReal() &&
  577.                 FastMath.abs(deltaI.getReal())     < thresholdAngles.getReal() &&
  578.                 FastMath.abs(deltaOmega.getReal()) < thresholdAngles.getReal() &&
  579.                 FastMath.abs(deltaRAAN.getReal())  < thresholdAngles.getReal() &&
  580.                 FastMath.abs(deltaAnom.getReal())  < thresholdAngles.getReal()) {
  581.                 return current;
  582.             }
  583.         }
  584.         throw new OrekitException(OrekitMessages.UNABLE_TO_COMPUTE_BROUWER_LYDDANE_MEAN_PARAMETERS, i);
  585.     }


  588.     /** {@inheritDoc} */
  589.     public FieldKeplerianOrbit<T> propagateOrbit(final FieldAbsoluteDate<T> date, final T[] parameters) {
  590.         // compute Cartesian parameters, taking derivatives into account
  591.         // to make sure velocity and acceleration are consistent
  592.         final FieldBLModel<T> current = models.get(date);
  593.         final FieldUnivariateDerivative2<T>[] propOrb_parameters = current.propagateParameters(date, parameters);
  594.         return new FieldKeplerianOrbit<T>(propOrb_parameters[0].getValue(), propOrb_parameters[1].getValue(),
  595.                                           propOrb_parameters[2].getValue(), propOrb_parameters[3].getValue(),
  596.                                           propOrb_parameters[4].getValue(), propOrb_parameters[5].getValue(),
  597.                                           PositionAngle.MEAN, current.mean.getFrame(), date, mu);
  598.     }

  600.     /**
  601.      * Get the value of the M2 drag parameter.
  602.      * @return the value of the M2 drag parameter
  603.      */
  604.     public double getM2() {
  605.         return M2Driver.getValue();
  606.     }

  608.     /** Local class for Brouwer-Lyddane model. */
  609.     private static class FieldBLModel<T extends CalculusFieldElement<T>> {

  611.         /** Mean orbit. */
  612.         private final FieldKeplerianOrbit<T> mean;

  614.         /** Constant mass. */
  615.         private final T mass;

  617.         // CHECKSTYLE: stop JavadocVariable check

  619.         // preprocessed values

  621.         // Constant for secular terms l'', g'', h''
  622.         // l standing for true anomaly, g for perigee argument and h for raan
  623.         private final T xnotDot;
  624.         private final T n;
  625.         private final T lt;
  626.         private final T gt;
  627.         private final T ht;


  630.         // Long periodT
  631.         private final T dei3sg;
  632.         private final T de2sg;
  633.         private final T deisg;
  634.         private final T de;


  637.         private final T dlgs2g;
  638.         private final T dlgc3g;
  639.         private final T dlgcg;


  642.         private final T dh2sgcg;
  643.         private final T dhsgcg;
  644.         private final T dhcg;


  647.         // Short perioTs
  648.         private final T aC;
  649.         private final T aCbis;
  650.         private final T ac2g2f;


  653.         private final T eC;
  654.         private final T ecf;
  655.         private final T e2cf;
  656.         private final T e3cf;
  657.         private final T ec2f2g;
  658.         private final T ecfc2f2g;
  659.         private final T e2cfc2f2g;
  660.         private final T e3cfc2f2g;
  661.         private final T ec2gf;
  662.         private final T ec2g3f;


  665.         private final T ide;
  666.         private final T isfs2f2g;
  667.         private final T icfc2f2g;
  668.         private final T ic2f2g;


  671.         private final T glf;
  672.         private final T gll;
  673.         private final T glsf;
  674.         private final T glosf;
  675.         private final T gls2f2g;
  676.         private final T gls2gf;
  677.         private final T glos2gf;
  678.         private final T gls2g3f;
  679.         private final T glos2g3f;


  682.         private final T hf;
  683.         private final T hl;
  684.         private final T hsf;
  685.         private final T hcfs2g2f;
  686.         private final T hs2g2f;
  687.         private final T hsfc2g2f;


  690.         private final T edls2g;
  691.         private final T edlcg;
  692.         private final T edlc3g;
  693.         private final T edlsf;
  694.         private final T edls2gf;
  695.         private final T edls2g3f;

  697.         // Drag terms
  698.         private final T aRate;
  699.         private final T eRate;

  701.         // CHECKSTYLE: resume JavadocVariable check

  703.         /** Create a model for specified mean orbit.
  704.          * @param mean mean Fieldorbit
  705.          * @param mass constant mass
  706.          * @param referenceRadius reference radius of the central body attraction model (m)
  707.          * @param mu central attraction coefficient (m³/s²)
  708.          * @param ck0 un-normalized zonal coefficients
  709.          */
  710.         FieldBLModel(final FieldKeplerianOrbit<T> mean, final T mass,
  711.                 final double referenceRadius, final T mu, final double[] ck0) {

  713.             this.mean = mean;
  714.             this.mass = mass;
  715.             final T one  = mass.getField().getOne();

  717.             final T app = mean.getA();
  718.             xnotDot = mu.divide(app).sqrt().divide(app);

  720.             // preliminary processing
  721.             final T q = app.divide(referenceRadius).reciprocal();
  722.             T ql = q.multiply(q);
  723.             final T y2 = ql.multiply(-0.5 * ck0[2]);

  725.             n = ((mean.getE().multiply(mean.getE()).negate()).add(1.0)).sqrt();
  726.             final T n2 = n.multiply(n);
  727.             final T n3 = n2.multiply(n);
  728.             final T n4 = n2.multiply(n2);
  729.             final T n6 = n4.multiply(n2);
  730.             final T n8 = n4.multiply(n4);
  731.             final T n10 = n8.multiply(n2);

  733.             final T yp2 = y2.divide(n4);
  734.             ql = ql.multiply(q);
  735.             final T yp3 = ql.multiply(ck0[3]).divide(n6);
  736.             ql = ql.multiply(q);
  737.             final T yp4 = ql.multiply(0.375 * ck0[4]).divide(n8);
  738.             ql = ql.multiply(q);
  739.             final T yp5 = ql.multiply(ck0[5]).divide(n10);


  742.             final FieldSinCos<T> sc = FastMath.sinCos(mean.getI());
  743.             final T sinI1 = sc.sin();
  744.             final T sinI2 = sinI1.multiply(sinI1);
  745.             final T cosI1 = sc.cos();
  746.             final T cosI2 = cosI1.multiply(cosI1);
  747.             final T cosI3 = cosI2.multiply(cosI1);
  748.             final T cosI4 = cosI2.multiply(cosI2);
  749.             final T cosI6 = cosI4.multiply(cosI2);
  750.             final T C5c2 = T2(cosI1).reciprocal();
  751.             final T C3c2 = cosI2.multiply(3.0).subtract(1.0);

  753.             final T epp = mean.getE();
  754.             final T epp2 = epp.multiply(epp);
  755.             final T epp3 = epp2.multiply(epp);
  756.             final T epp4 = epp2.multiply(epp2);

  758.             if (epp.getReal() >= 1) {
  759.                 // Only for elliptical (e < 1) orbits
  760.                 throw new OrekitException(OrekitMessages.TOO_LARGE_ECCENTRICITY_FOR_PROPAGATION_MODEL,
  761.                                           mean.getE().getReal());
  762.             }

  764.             // secular multiplicative
  765.             lt = one.add(yp2.multiply(n).multiply(C3c2).multiply(1.5)).
  766.                      add(yp2.multiply(0.09375).multiply(yp2).multiply(n).multiply(n2.multiply(25.0).add(n.multiply(16.0)).add(-15.0).
  767.                              add(cosI2.multiply(n2.multiply(-90.0).add(n.multiply(-96.0)).add(30.0))).
  768.                              add(cosI4.multiply(n2.multiply(25.0).add(n.multiply(144.0)).add(105.0))))).
  769.                      add(yp4.multiply(0.9375).multiply(n).multiply(epp2).multiply(cosI4.multiply(35.0).add(cosI2.multiply(-30.0)).add(3.0)));

  771.             gt = yp2.multiply(-1.5).multiply(C5c2).
  772.                      add(yp2.multiply(0.09375).multiply(yp2).multiply(n2.multiply(25.0).add(n.multiply(24.0)).add(-35.0).
  773.                             add(cosI2.multiply(n2.multiply(-126.0).add(n.multiply(-192.0)).add(90.0))).
  774.                             add(cosI4.multiply(n2.multiply(45.0).add(n.multiply(360.0)).add(385.0))))).
  775.                      add(yp4.multiply(0.3125).multiply(n2.multiply(-9.0).add(21.0).
  776.                             add(cosI2.multiply(n2.multiply(126.0).add(-270.0))).
  777.                             add(cosI4.multiply(n2.multiply(-189.0).add(385.0)))));

  779.             ht = yp2.multiply(-3.0).multiply(cosI1).
  780.                      add(yp2.multiply(0.375).multiply(yp2).multiply(cosI1.multiply(n2.multiply(9.0).add(n.multiply(12.0)).add(-5.0)).
  781.                                                                     add(cosI3.multiply(n2.multiply(-5.0).add(n.multiply(-36.0)).add(-35.0))))).
  782.                      add(yp4.multiply(1.25).multiply(cosI1).multiply(n2.multiply(-3.0).add(5.0)).multiply(cosI2.multiply(-7.0).add(3.0)));

  784.             final T cA = one.subtract(cosI2.multiply(11.0)).subtract(cosI4.multiply(40.0).divide(C5c2));
  785.             final T cB = one.subtract(cosI2.multiply(3.0)).subtract(cosI4.multiply(8.0).divide(C5c2));
  786.             final T cC = one.subtract(cosI2.multiply(9.0)).subtract(cosI4.multiply(24.0).divide(C5c2));
  787.             final T cD = one.subtract(cosI2.multiply(5.0)).subtract(cosI4.multiply(16.0).divide(C5c2));

  789.             final T qyp2_4 = yp2.multiply(3.0).multiply(yp2).multiply(cA).
  790.                              subtract(yp4.multiply(10.0).multiply(cB));
  791.             final T qyp52 = cosI1.multiply(epp3).multiply(cD.multiply(0.5).divide(sinI1).
  792.                                                           add(sinI1.multiply(cosI4.divide(C5c2).divide(C5c2).multiply(80.0).
  793.                                                                              add(cosI2.divide(C5c2).multiply(32.0).
  794.                                                                              add(5.0)))));
  795.             final T qyp22 = epp2.add(2.0).subtract(epp2.multiply(3.0).add(2.0).multiply(11.0).multiply(cosI2)).
  796.                             subtract(epp2.multiply(5.0).add(2.0).multiply(40.0).multiply(cosI4.divide(C5c2))).
  797.                             subtract(epp2.multiply(400.0).multiply(cosI6).divide(C5c2).divide(C5c2));
  798.             final T qyp42 = one.divide(5.0).multiply(qyp22.
  799.                                                      add(one.multiply(4.0).multiply(epp2.
  800.                                                                                     add(2.0).
  801.                                                                                     subtract(cosI2.multiply(epp2.multiply(3.0).add(2.0))))));
  802.             final T qyp52bis = cosI1.multiply(sinI1).multiply(epp).multiply(epp2.multiply(3.0).add(4.0)).
  803.                                                                    multiply(cosI4.divide(C5c2).divide(C5c2).multiply(40.0).
  804.                                                                             add(cosI2.divide(C5c2).multiply(16.0)).
  805.                                                                             add(3.0));
  806.            // long periodic multiplicative
  807.             dei3sg =  yp5.divide(yp2).multiply(35.0 / 96.0).multiply(epp2).multiply(n2).multiply(cD).multiply(sinI1);
  808.             de2sg = qyp2_4.divide(yp2).multiply(epp).multiply(n2).multiply(-1.0 / 12.0);
  809.             deisg = sinI1.multiply(yp5.multiply(-35.0 / 128.0).divide(yp2).multiply(epp2).multiply(n2).multiply(cD).
  810.                             add(n2.multiply(0.25).divide(yp2).multiply(yp3.add(yp5.multiply(5.0 / 16.0).multiply(epp2.multiply(3.0).add(4.0)).multiply(cC)))));
  811.             de = epp2.multiply(n2).multiply(qyp2_4).divide(24.0).divide(yp2);

  813.             final T qyp52quotient = epp.multiply(epp4.multiply(81.0).add(-32.0)).divide(n.multiply(epp2.multiply(9.0).add(4.0)).add(epp2.multiply(3.0)).add(4.0));
  814.             dlgs2g = yp2.multiply(48.0).reciprocal().multiply(yp2.multiply(-3.0).multiply(yp2).multiply(qyp22).
  815.                             add(yp4.multiply(10.0).multiply(qyp42))).
  816.                             add(n3.divide(yp2).multiply(qyp2_4).divide(24.0));
  817.             dlgc3g =  yp5.multiply(35.0 / 384.0).divide(yp2).multiply(n3).multiply(epp).multiply(cD).multiply(sinI1).
  818.                             add(yp5.multiply(35.0 / 1152.0).divide(yp2).multiply(qyp52.multiply(2.0).multiply(cosI1).subtract(epp.multiply(cD).multiply(sinI1).multiply(epp2.multiply(2.0).add(3.0)))));
  819.             dlgcg = yp3.negate().multiply(cosI2).multiply(epp).divide(yp2.multiply(sinI1).multiply(4.0)).
  820.                     add(yp5.divide(yp2).multiply(0.078125).multiply(cC).multiply(cosI2.divide(sinI1).multiply(epp.negate()).multiply(epp2.multiply(3.0).add(4.0)).
  821.                                                                     add(sinI1.multiply(epp2).multiply(epp2.multiply(9.0).add(26.0)))).
  822.                     subtract(yp5.divide(yp2).multiply(0.46875).multiply(qyp52bis).multiply(cosI1)).
  823.                     add(yp3.divide(yp2).multiply(0.25).multiply(sinI1).multiply(epp).divide(n3.add(1.0)).multiply(epp2.multiply(epp2.subtract(3.0)).add(3.0))).
  824.                     add(yp5.divide(yp2).multiply(0.078125).multiply(n2).multiply(cC).multiply(qyp52quotient).multiply(sinI1)));
  825.             final T qyp24 = yp2.multiply(yp2).multiply(3.0).multiply(cosI4.divide(sinI2).multiply(200.0).add(cosI2.divide(sinI1).multiply(80.0)).add(11.0)).
  826.                             subtract(yp4.multiply(10.0).multiply(cosI4.divide(sinI2).multiply(40.0).add(cosI2.divide(sinI1).multiply(16.0)).add(3.0)));
  827.             dh2sgcg = yp5.divide(yp2).multiply(qyp52).multiply(35.0 / 144.0);
  828.             dhsgcg = qyp24.multiply(cosI1).multiply(epp2.negate()).divide(yp2.multiply(12.0));
  829.             dhcg = yp5.divide(yp2).multiply(qyp52).multiply(-35.0 / 576.0).
  830.                    add(cosI1.multiply(epp).divide(yp2.multiply(sinI1).multiply(4.0)).multiply(yp3.add(yp5.multiply(0.3125).multiply(cC).multiply(epp2.multiply(3.0).add(4.0))))).
  831.                    add(yp5.multiply(qyp52bis).multiply(1.875).divide(yp2.multiply(4.0)));

  833.             // short periodic multiplicative
  834.             aC = yp2.negate().multiply(C3c2).multiply(app).divide(n3);
  835.             aCbis = y2.multiply(app).multiply(C3c2);
  836.             ac2g2f = y2.multiply(app).multiply(sinI2).multiply(3.0);

  838.             T qe = y2.multiply(C3c2).multiply(0.5).multiply(n2).divide(n6);
  839.             eC = qe.multiply(epp).divide(n3.add(1.0)).multiply(epp2.multiply(epp2.subtract(3.0)).add(3.0));
  840.             ecf = qe.multiply(3.0);
  841.             e2cf = qe.multiply(3.0).multiply(epp);
  842.             e3cf = qe.multiply(epp2);
  843.             qe = y2.multiply(0.5).multiply(n2).multiply(3.0).multiply(cosI2.negate().add(1.0)).divide(n6);
  844.             ec2f2g = qe.multiply(epp);
  845.             ecfc2f2g = qe.multiply(3.0);
  846.             e2cfc2f2g = qe.multiply(3.0).multiply(epp);
  847.             e3cfc2f2g = qe.multiply(epp2);
  848.             qe = yp2.multiply(-0.5).multiply(n2).multiply(cosI2.negate().add(1.0));
  849.             ec2gf = qe.multiply(3.0);
  850.             ec2g3f = qe;

  852.             T qi = yp2.multiply(epp).multiply(cosI1).multiply(sinI1);
  853.             ide = cosI1.multiply(epp.negate()).divide(sinI1.multiply(n2));
  854.             isfs2f2g = qi;
  855.             icfc2f2g = qi.multiply(2.0);
  856.             qi = yp2.multiply(cosI1).multiply(sinI1);
  857.             ic2f2g = qi.multiply(1.5);

  859.             T qgl1 = yp2.multiply(0.25);
  860.             T qgl2 =  yp2.multiply(epp).multiply(n2).multiply(0.25).divide(n.add(1.0));
  861.             glf = qgl1.multiply(C5c2).multiply(-6.0);
  862.             gll = qgl1.multiply(C5c2).multiply(6.0);
  863.             glsf = qgl1.multiply(C5c2).multiply(-6.0).multiply(epp).
  864.                    add(qgl2.multiply(C3c2).multiply(2.0));
  865.             glosf = qgl2.multiply(C3c2).multiply(2.0);
  866.             qgl1 = qgl1.multiply(cosI2.multiply(-5.0).add(3.0));
  867.             qgl2 = qgl2.multiply(3.0).multiply(cosI2.negate().add(1.0));
  868.             gls2f2g = qgl1.multiply(3.0);
  869.             gls2gf = qgl1.multiply(3.0).multiply(epp).
  870.                      add(qgl2);
  871.             glos2gf = qgl2.negate();
  872.             gls2g3f = qgl1.multiply(epp).
  873.                       add(qgl2.multiply(1.0 / 3.0));
  874.             glos2g3f = qgl2;

  876.             final T qh = yp2.multiply(cosI1).multiply(3.0);
  877.             hf = qh.negate();
  878.             hl = qh;
  879.             hsf = qh.multiply(epp).negate();
  880.             hcfs2g2f = yp2.multiply(cosI1).multiply(epp).multiply(2.0);
  881.             hs2g2f = yp2.multiply(cosI1).multiply(1.5);
  882.             hsfc2g2f = yp2.multiply(cosI1).multiply(epp).negate();

  884.             final T qedl = yp2.multiply(n3).multiply(-0.25);
  885.             edls2g = qyp2_4.multiply(1.0 / 24.0).multiply(epp).multiply(n3).divide(yp2);
  886.             edlcg = yp3.divide(yp2).multiply(-0.25).multiply(n3).multiply(sinI1).
  887.                     subtract(yp5.divide(yp2).multiply(0.078125).multiply(n3).multiply(sinI1).multiply(cC).multiply(epp2.multiply(9.0).add(4.0)));
  888.             edlc3g = yp5.divide(yp2).multiply(n3).multiply(epp2).multiply(cD).multiply(sinI1).multiply(35.0 / 384.0);
  889.             edlsf = qedl.multiply(C3c2).multiply(2.0);
  890.             edls2gf = qedl.multiply(3.0).multiply(cosI2.negate().add(1.0));
  891.             edls2g3f = qedl.multiply(1.0 / 3.0);

  893.             // secular rates of the mean semi-major axis and eccentricity
  894.             // Eq. 2.41 and Eq. 2.45 of Phipps' 1992 thesis
  895.             aRate = app.multiply(-4.0).divide(xnotDot.multiply(3.0));
  896.             eRate = epp.multiply(n).multiply(n).multiply(-4.0).divide(xnotDot.multiply(3.0));

  898.         }

  900.         /**
  901.          * Accurate computation of E - e sin(E).
  902.          *
  903.          * @param E eccentric anomaly
  904.          * @return E - e sin(E)
  905.          */
  906.         private FieldUnivariateDerivative2<T> eMeSinE(final FieldUnivariateDerivative2<T> E) {
  907.             FieldUnivariateDerivative2<T> x = E.sin().multiply(mean.getE().negate().add(1.0));
  908.             final FieldUnivariateDerivative2<T> mE2 = E.negate().multiply(E);
  909.             FieldUnivariateDerivative2<T> term = E;
  910.             FieldUnivariateDerivative2<T> d    = E.getField().getZero();
  911.             // the inequality test below IS intentional and should NOT be replaced by a check with a small tolerance
  912.             for (FieldUnivariateDerivative2<T> x0 = d.add(Double.NaN); !Double.valueOf(x.getValue().getReal()).equals(Double.valueOf(x0.getValue().getReal()));) {
  913.                 d = d.add(2);
  914.                 term = term.multiply(mE2.divide(d.multiply(d.add(1))));
  915.                 x0 = x;
  916.                 x = x.subtract(term);
  917.             }
  918.             return x;
  919.         }

  921.         /**
  922.          * Gets eccentric anomaly from mean anomaly.
  923.          * <p>The algorithm used to solve the Kepler equation has been published in:
  924.          * "Procedures for  solving Kepler's Equation", A. W. Odell and R. H. Gooding,
  925.          * Celestial Mechanics 38 (1986) 307-334</p>
  926.          * <p>It has been copied from the OREKIT library (KeplerianOrbit class).</p>
  927.          *
  928.          * @param mk the mean anomaly (rad)
  929.          * @return the eccentric anomaly (rad)
  930.          */
  931.         private FieldUnivariateDerivative2<T> getEccentricAnomaly(final FieldUnivariateDerivative2<T> mk) {
  932.             final double k1 = 3 * FastMath.PI + 2;
  933.             final double k2 = FastMath.PI - 1;
  934.             final double k3 = 6 * FastMath.PI - 1;
  935.             final double A  = 3.0 * k2 * k2 / k1;
  936.             final double B  = k3 * k3 / (6.0 * k1);

  938.             final T zero = mean.getE().getField().getZero();

  940.             // reduce M to [-PI PI] interval
  941.             final FieldUnivariateDerivative2<T> reducedM = new FieldUnivariateDerivative2<T>(MathUtils.normalizeAngle(mk.getValue(), zero ),
  942.                                                                              mk.getFirstDerivative(),
  943.                                                                              mk.getSecondDerivative());

  945.             // compute start value according to A. W. Odell and R. H. Gooding S12 starter
  946.             FieldUnivariateDerivative2<T> ek;
  947.             if (reducedM.getValue().abs().getReal() < 1.0 / 6.0) {
  948.                 if (reducedM.getValue().abs().getReal() < Precision.SAFE_MIN) {
  949.                     // this is an Orekit change to the S12 starter.
  950.                     // If reducedM is 0.0, the derivative of cbrt is infinite which induces NaN appearing later in
  951.                     // the computation. As in this case E and M are almost equal, we initialize ek with reducedM
  952.                     ek = reducedM;
  953.                 } else {
  954.                     // this is the standard S12 starter
  955.                     ek = reducedM.add(reducedM.multiply(6).cbrt().subtract(reducedM).multiply(mean.getE()));
  956.                 }
  957.             } else {
  958.                 if (reducedM.getValue().getReal() < 0) {
  959.                     final FieldUnivariateDerivative2<T> w = reducedM.add(FastMath.PI);
  960.                     ek = reducedM.add(w.multiply(-A).divide(w.subtract(B)).subtract(FastMath.PI).subtract(reducedM).multiply(mean.getE()));
  961.                 } else {
  962.                     final FieldUnivariateDerivative2<T> minusW = reducedM.subtract(FastMath.PI);
  963.                     ek = reducedM.add(minusW.multiply(A).divide(minusW.add(B)).add(FastMath.PI).subtract(reducedM).multiply(mean.getE()));
  964.                 }
  965.             }

  967.             final T e1 = mean.getE().negate().add(1.0);
  968.             final boolean noCancellationRisk = (e1.add(ek.getValue().multiply(ek.getValue())).getReal() / 6) >= 0.1;

  970.             // perform two iterations, each consisting of one Halley step and one Newton-Raphson step
  971.             for (int j = 0; j < 2; ++j) {
  972.                 final FieldUnivariateDerivative2<T> f;
  973.                 FieldUnivariateDerivative2<T> fd;
  974.                 final FieldUnivariateDerivative2<T> fdd  = ek.sin().multiply(mean.getE());
  975.                 final FieldUnivariateDerivative2<T> fddd = ek.cos().multiply(mean.getE());
  976.                 if (noCancellationRisk) {
  977.                     f  = ek.subtract(fdd).subtract(reducedM);
  978.                     fd = fddd.subtract(1).negate();
  979.                 } else {
  980.                     f  = eMeSinE(ek).subtract(reducedM);
  981.                     final FieldUnivariateDerivative2<T> s = ek.multiply(0.5).sin();
  982.                     fd = s.multiply(s).multiply(mean.getE().multiply(2.0)).add(e1);
  983.                 }
  984.                 final FieldUnivariateDerivative2<T> dee = f.multiply(fd).divide(f.multiply(0.5).multiply(fdd).subtract(fd.multiply(fd)));

  986.                 // update eccentric anomaly, using expressions that limit underflow problems
  987.                 final FieldUnivariateDerivative2<T> w = fd.add(dee.multiply(0.5).multiply(fdd.add(dee.multiply(fdd).divide(3))));
  988.                 fd = fd.add(dee.multiply(fdd.add(dee.multiply(0.5).multiply(fdd))));
  989.                 ek = ek.subtract(f.subtract(dee.multiply(fd.subtract(w))).divide(fd));
  990.             }

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

  995.             // Returns the eccentric anomaly
  996.             return ek;
  997.         }

  999.         /**
  1000.          * This method is used in Brouwer-Lyddane model to avoid singularity at the
  1001.          * critical inclination (i = 63.4°).
  1002.          * <p>
  1003.          * This method, based on Warren Phipps's 1992 thesis (Eq. 2.47 and 2.48),
  1004.          * approximate the factor (1.0 - 5.0 * cos²(inc))^-1 (causing the singularity)
  1005.          * by a function, named T2 in the thesis.
  1006.          * </p>
  1007.          * @param cosInc cosine of the mean inclination
  1008.          * @return an approximation of (1.0 - 5.0 * cos²(inc))^-1 term
  1009.          */
  1010.         private T T2(final T cosInc) {

  1012.             // X = (1.0 - 5.0 * cos²(inc))
  1013.             final T x  = cosInc.multiply(cosInc).multiply(-5.0).add(1.0);
  1014.             final T x2 = x.multiply(x);

  1016.             // Eq. 2.48
  1017.             T sum = x.getField().getZero();
  1018.             for (int i = 0; i <= 12; i++) {
  1019.                 final double sign = i % 2 == 0 ? +1.0 : -1.0;
  1020.                 sum = sum.add(FastMath.pow(x2, i).multiply(FastMath.pow(BETA, i)).multiply(sign).divide(CombinatoricsUtils.factorialDouble(i + 1)));
  1021.             }

  1023.             // Right term of equation 2.47
  1024.             T product = x.getField().getOne();
  1025.             for (int i = 0; i <= 10; i++) {
  1026.                 product = product.multiply(FastMath.exp(x2.multiply(BETA).multiply(FastMath.scalb(-1.0, i))).add(1.0));
  1027.             }

  1029.             // Return (Eq. 2.47)
  1030.             return x.multiply(BETA).multiply(sum).multiply(product);

  1032.         }

  1034.         /** Extrapolate an orbit up to a specific target date.
  1035.          * @param date target date for the orbit
  1036.          * @param parameters model parameters
  1037.          * @return propagated parameters
  1038.          */
  1039.         public FieldUnivariateDerivative2<T>[] propagateParameters(final FieldAbsoluteDate<T> date, final T[] parameters) {

  1041.             // Field
  1042.             final Field<T> field = date.getField();
  1043.             final T one  = field.getOne();
  1044.             final T zero = field.getZero();

  1046.             // Empirical drag coefficient M2
  1047.             final T m2 = parameters[0];

  1049.             // Keplerian evolution
  1050.             final FieldUnivariateDerivative2<T> dt = new FieldUnivariateDerivative2<>(date.durationFrom(mean.getDate()), one, zero);
  1051.             final FieldUnivariateDerivative2<T> xnot = dt.multiply(xnotDot);

  1053.             //____________________________________
  1054.             // secular effects

  1056.             // mean mean anomaly
  1057.             final FieldUnivariateDerivative2<T> dtM2  = dt.multiply(m2);
  1058.             final FieldUnivariateDerivative2<T> dt2M2 = dt.multiply(dtM2);
  1059.             final FieldUnivariateDerivative2<T> lpp = new FieldUnivariateDerivative2<T>(MathUtils.normalizeAngle(mean.getMeanAnomaly().add(lt.multiply(xnot.getValue())).add(dt2M2.getValue()),
  1060.                                                                                         one.getPi()),
  1061.                                                                                         lt.multiply(xnotDot).add(dtM2.multiply(2.0).getValue()),
  1062.                                                                                         m2.multiply(2.0));
  1063.             // mean argument of perigee
  1064.             final FieldUnivariateDerivative2<T> gpp = new FieldUnivariateDerivative2<T>(MathUtils.normalizeAngle(mean.getPerigeeArgument().add(gt.multiply(xnot.getValue())),
  1065.                                                                                         one.getPi()),
  1066.                                                                                         gt.multiply(xnotDot),
  1067.                                                                                         zero);
  1068.             // mean longitude of ascending node
  1069.             final FieldUnivariateDerivative2<T> hpp = new FieldUnivariateDerivative2<T>(MathUtils.normalizeAngle(mean.getRightAscensionOfAscendingNode().add(ht.multiply(xnot.getValue())),
  1070.                                                                                         one.getPi()),
  1071.                                                                                         ht.multiply(xnotDot),
  1072.                                                                                         zero);

  1074.             // ________________________________________________
  1075.             // secular rates of the mean semi-major axis and eccentricity

  1077.             // semi-major axis
  1078.             final FieldUnivariateDerivative2<T> appDrag = dt.multiply(aRate.multiply(m2));

  1080.             // eccentricity
  1081.             final FieldUnivariateDerivative2<T> eppDrag = dt.multiply(eRate.multiply(m2));

  1083.             //____________________________________
  1084.             // Long periodical terms
  1085.             final FieldUnivariateDerivative2<T> cg1 = gpp.cos();
  1086.             final FieldUnivariateDerivative2<T> sg1 = gpp.sin();
  1087.             final FieldUnivariateDerivative2<T> c2g = cg1.multiply(cg1).subtract(sg1.multiply(sg1));
  1088.             final FieldUnivariateDerivative2<T> s2g = cg1.multiply(sg1).add(sg1.multiply(cg1));
  1089.             final FieldUnivariateDerivative2<T> c3g = c2g.multiply(cg1).subtract(s2g.multiply(sg1));
  1090.             final FieldUnivariateDerivative2<T> sg2 = sg1.multiply(sg1);
  1091.             final FieldUnivariateDerivative2<T> sg3 = sg1.multiply(sg2);


  1094.             // de eccentricity
  1095.             final FieldUnivariateDerivative2<T> d1e = sg3.multiply(dei3sg).
  1096.                                                add(sg1.multiply(deisg)).
  1097.                                                add(sg2.multiply(de2sg)).
  1098.                                                add(de);

  1100.             // l' + g'
  1101.             final FieldUnivariateDerivative2<T> lp_p_gp = s2g.multiply(dlgs2g).
  1102.                                                add(c3g.multiply(dlgc3g)).
  1103.                                                add(cg1.multiply(dlgcg)).
  1104.                                                add(lpp).
  1105.                                                add(gpp);

  1107.             // h'
  1108.             final FieldUnivariateDerivative2<T> hp = sg2.multiply(cg1).multiply(dh2sgcg).
  1109.                                                add(sg1.multiply(cg1).multiply(dhsgcg)).
  1110.                                                add(cg1.multiply(dhcg)).
  1111.                                                add(hpp);

  1113.             // Short periodical terms
  1114.             // eccentric anomaly
  1115.             final FieldUnivariateDerivative2<T> Ep = getEccentricAnomaly(lpp);
  1116.             final FieldUnivariateDerivative2<T> cf1 = (Ep.cos().subtract(mean.getE())).divide(Ep.cos().multiply(mean.getE().negate()).add(1.0));
  1117.             final FieldUnivariateDerivative2<T> sf1 = (Ep.sin().multiply(n)).divide(Ep.cos().multiply(mean.getE().negate()).add(1.0));
  1118.             final FieldUnivariateDerivative2<T> f = FastMath.atan2(sf1, cf1);

  1120.             final FieldUnivariateDerivative2<T> c2f = cf1.multiply(cf1).subtract(sf1.multiply(sf1));
  1121.             final FieldUnivariateDerivative2<T> s2f = cf1.multiply(sf1).add(sf1.multiply(cf1));
  1122.             final FieldUnivariateDerivative2<T> c3f = c2f.multiply(cf1).subtract(s2f.multiply(sf1));
  1123.             final FieldUnivariateDerivative2<T> s3f = c2f.multiply(sf1).add(s2f.multiply(cf1));
  1124.             final FieldUnivariateDerivative2<T> cf2 = cf1.multiply(cf1);
  1125.             final FieldUnivariateDerivative2<T> cf3 = cf1.multiply(cf2);

  1127.             final FieldUnivariateDerivative2<T> c2g1f = cf1.multiply(c2g).subtract(sf1.multiply(s2g));
  1128.             final FieldUnivariateDerivative2<T> c2g2f = c2f.multiply(c2g).subtract(s2f.multiply(s2g));
  1129.             final FieldUnivariateDerivative2<T> c2g3f = c3f.multiply(c2g).subtract(s3f.multiply(s2g));
  1130.             final FieldUnivariateDerivative2<T> s2g1f = cf1.multiply(s2g).add(c2g.multiply(sf1));
  1131.             final FieldUnivariateDerivative2<T> s2g2f = c2f.multiply(s2g).add(c2g.multiply(s2f));
  1132.             final FieldUnivariateDerivative2<T> s2g3f = c3f.multiply(s2g).add(c2g.multiply(s3f));

  1134.             final FieldUnivariateDerivative2<T> eE = (Ep.cos().multiply(mean.getE().negate()).add(1.0)).reciprocal();
  1135.             final FieldUnivariateDerivative2<T> eE3 = eE.multiply(eE).multiply(eE);
  1136.             final FieldUnivariateDerivative2<T> sigma = eE.multiply(n.multiply(n)).multiply(eE).add(eE);

  1138.             // Semi-major axis
  1139.             final FieldUnivariateDerivative2<T> a = eE3.multiply(aCbis).add(appDrag.add(mean.getA())).
  1140.                                             add(aC).
  1141.                                             add(eE3.multiply(c2g2f).multiply(ac2g2f));

  1143.             // Eccentricity
  1144.             final FieldUnivariateDerivative2<T> e = d1e.add(eppDrag.add(mean.getE())).
  1145.                                             add(eC).
  1146.                                             add(cf1.multiply(ecf)).
  1147.                                             add(cf2.multiply(e2cf)).
  1148.                                             add(cf3.multiply(e3cf)).
  1149.                                             add(c2g2f.multiply(ec2f2g)).
  1150.                                             add(c2g2f.multiply(cf1).multiply(ecfc2f2g)).
  1151.                                             add(c2g2f.multiply(cf2).multiply(e2cfc2f2g)).
  1152.                                             add(c2g2f.multiply(cf3).multiply(e3cfc2f2g)).
  1153.                                             add(c2g1f.multiply(ec2gf)).
  1154.                                             add(c2g3f.multiply(ec2g3f));

  1156.             // Inclination
  1157.             final FieldUnivariateDerivative2<T> i = d1e.multiply(ide).
  1158.                                             add(mean.getI()).
  1159.                                             add(sf1.multiply(s2g2f.multiply(isfs2f2g))).
  1160.                                             add(cf1.multiply(c2g2f.multiply(icfc2f2g))).
  1161.                                             add(c2g2f.multiply(ic2f2g));

  1163.             // Argument of perigee + True anomaly
  1164.             final FieldUnivariateDerivative2<T> g_p_l = lp_p_gp.add(f.multiply(glf)).
  1165.                                              add(lpp.multiply(gll)).
  1166.                                              add(sf1.multiply(glsf)).
  1167.                                              add(sigma.multiply(sf1).multiply(glosf)).
  1168.                                              add(s2g2f.multiply(gls2f2g)).
  1169.                                              add(s2g1f.multiply(gls2gf)).
  1170.                                              add(sigma.multiply(s2g1f).multiply(glos2gf)).
  1171.                                              add(s2g3f.multiply(gls2g3f)).
  1172.                                              add(sigma.multiply(s2g3f).multiply(glos2g3f));


  1175.             // Longitude of ascending node
  1176.             final FieldUnivariateDerivative2<T> h = hp.add(f.multiply(hf)).
  1177.                                             add(lpp.multiply(hl)).
  1178.                                             add(sf1.multiply(hsf)).
  1179.                                             add(cf1.multiply(s2g2f).multiply(hcfs2g2f)).
  1180.                                             add(s2g2f.multiply(hs2g2f)).
  1181.                                             add(c2g2f.multiply(sf1).multiply(hsfc2g2f));

  1183.             final FieldUnivariateDerivative2<T> edl = s2g.multiply(edls2g).
  1184.                                             add(cg1.multiply(edlcg)).
  1185.                                             add(c3g.multiply(edlc3g)).
  1186.                                             add(sf1.multiply(edlsf)).
  1187.                                             add(s2g1f.multiply(edls2gf)).
  1188.                                             add(s2g3f.multiply(edls2g3f)).
  1189.                                             add(sf1.multiply(sigma).multiply(edlsf)).
  1190.                                             add(s2g1f.multiply(sigma).multiply(edls2gf.negate())).
  1191.                                             add(s2g3f.multiply(sigma).multiply(edls2g3f.multiply(3.0)));

  1193.             final FieldUnivariateDerivative2<T> A = e.multiply(lpp.cos()).subtract(edl.multiply(lpp.sin()));
  1194.             final FieldUnivariateDerivative2<T> B = e.multiply(lpp.sin()).add(edl.multiply(lpp.cos()));

  1196.             // True anomaly
  1197.             final FieldUnivariateDerivative2<T> l = FastMath.atan2(B, A);

  1199.             // Argument of perigee
  1200.             final FieldUnivariateDerivative2<T> g = g_p_l.subtract(l);

  1202.             final FieldUnivariateDerivative2<T>[] FTD = MathArrays.buildArray(g.getField(), 6);
  1203.             FTD[0] = a;
  1204.             FTD[1] = e;
  1205.             FTD[2] = i;
  1206.             FTD[3] = g;
  1207.             FTD[4] = h;
  1208.             FTD[5] = l;
  1209.             return FTD;
  1210.         }
  1211.     }

  1213.     /** {@inheritDoc} */
  1214.     @Override
  1215.     protected T getMass(final FieldAbsoluteDate<T> date) {
  1216.         return models.get(date).mass;
  1217.     }

  1219.     /** {@inheritDoc} */
  1220.     @Override
  1221.     protected List<ParameterDriver> getParametersDrivers() {
  1222.         return Collections.singletonList(M2Driver);
  1223.     }

  1225. }
Created with JaCoCo 0.8.7.202105040129