FieldNumericalPropagator.java

  1. /* Copyright 2002-2020 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.numerical;

  19. import java.util.ArrayList;
  20. import java.util.Arrays;
  21. import java.util.Collections;
  22. import java.util.List;

  24. import org.hipparchus.Field;
  25. import org.hipparchus.RealFieldElement;
  26. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  27. import org.hipparchus.ode.FieldODEIntegrator;
  28. import org.hipparchus.util.FastMath;
  29. import org.hipparchus.util.MathArrays;
  30. import org.orekit.annotation.DefaultDataContext;
  31. import org.orekit.attitudes.AttitudeProvider;
  32. import org.orekit.attitudes.FieldAttitude;
  33. import org.orekit.data.DataContext;
  34. import org.orekit.errors.OrekitException;
  35. import org.orekit.errors.OrekitIllegalArgumentException;
  36. import org.orekit.errors.OrekitInternalError;
  37. import org.orekit.errors.OrekitMessages;
  38. import org.orekit.forces.ForceModel;
  39. import org.orekit.forces.gravity.NewtonianAttraction;
  40. import org.orekit.frames.Frame;
  41. import org.orekit.orbits.FieldOrbit;
  42. import org.orekit.orbits.OrbitType;
  43. import org.orekit.orbits.PositionAngle;
  44. import org.orekit.propagation.FieldSpacecraftState;
  45. import org.orekit.propagation.PropagationType;
  46. import org.orekit.propagation.Propagator;
  47. import org.orekit.propagation.SpacecraftState;
  48. import org.orekit.propagation.events.FieldEventDetector;
  49. import org.orekit.propagation.integration.FieldAbstractIntegratedPropagator;
  50. import org.orekit.propagation.integration.FieldStateMapper;
  51. import org.orekit.time.AbsoluteDate;
  52. import org.orekit.time.FieldAbsoluteDate;
  53. import org.orekit.utils.FieldAbsolutePVCoordinates;
  54. import org.orekit.utils.FieldPVCoordinates;
  55. import org.orekit.utils.ParameterDriver;
  56. import org.orekit.utils.ParameterObserver;
  57. import org.orekit.utils.TimeStampedFieldPVCoordinates;

  59. /** This class propagates {@link org.orekit.orbits.FieldOrbit orbits} using
  60.  * numerical integration.
  61.  * <p>Numerical propagation is much more accurate than analytical propagation
  62.  * like for example {@link org.orekit.propagation.analytical.KeplerianPropagator
  63.  * Keplerian} or {@link org.orekit.propagation.analytical.EcksteinHechlerPropagator
  64.  * Eckstein-Hechler}, but requires a few more steps to set up to be used properly.
  65.  * Whereas analytical propagators are configured only thanks to their various
  66.  * constructors and can be used immediately after construction, numerical propagators
  67.  * configuration involve setting several parameters between construction time
  68.  * and propagation time.</p>
  69.  * <p>The configuration parameters that can be set are:</p>
  70.  * <ul>
  71.  *   <li>the initial spacecraft state ({@link #setInitialState(FieldSpacecraftState)})</li>
  72.  *   <li>the central attraction coefficient ({@link #setMu(RealFieldElement)})</li>
  73.  *   <li>the various force models ({@link #addForceModel(ForceModel)},
  74.  *   {@link #removeForceModels()})</li>
  75.  *   <li>the {@link OrbitType type} of orbital parameters to be used for propagation
  76.  *   ({@link #setOrbitType(OrbitType)}),
  77.  *   <li>the {@link PositionAngle type} of position angle to be used in orbital parameters
  78.  *   to be used for propagation where it is relevant ({@link
  79.  *   #setPositionAngleType(PositionAngle)}),
  80.  *   <li>whether {@link org.orekit.propagation.integration.FieldAdditionalEquations additional equations}
  81.  *   should be propagated along with orbital state
  82.  *   ({@link #addAdditionalEquations(org.orekit.propagation.integration.FieldAdditionalEquations)}),
  83.  *   <li>the discrete events that should be triggered during propagation
  84.  *   ({@link #addEventDetector(FieldEventDetector)},
  85.  *   {@link #clearEventsDetectors()})</li>
  86.  *   <li>the binding logic with the rest of the application ({@link #setSlaveMode()},
  87.  *   {@link #setMasterMode(RealFieldElement, org.orekit.propagation.sampling.FieldOrekitFixedStepHandler)},
  88.  *   {@link #setMasterMode(org.orekit.propagation.sampling.FieldOrekitStepHandler)},
  89.  *   {@link #setEphemerisMode()}, {@link #getGeneratedEphemeris()})</li>
  90.  * </ul>
  91.  * <p>From these configuration parameters, only the initial state is mandatory. The default
  92.  * propagation settings are in {@link OrbitType#EQUINOCTIAL equinoctial} parameters with
  93.  * {@link PositionAngle#TRUE true} longitude argument. If the central attraction coefficient
  94.  * is not explicitly specified, the one used to define the initial orbit will be used.
  95.  * However, specifying only the initial state and perhaps the central attraction coefficient
  96.  * would mean the propagator would use only Keplerian forces. In this case, the simpler {@link
  97.  * org.orekit.propagation.analytical.KeplerianPropagator KeplerianPropagator} class would
  98.  * perhaps be more effective.</p>
  99.  * <p>The underlying numerical integrator set up in the constructor may also have its own
  100.  * configuration parameters. Typical configuration parameters for adaptive stepsize integrators
  101.  * are the min, max and perhaps start step size as well as the absolute and/or relative errors
  102.  * thresholds.</p>
  103.  * <p>The state that is seen by the integrator is a simple seven elements double array.
  104.  * The six first elements are either:
  105.  * <ul>
  106.  *   <li>the {@link org.orekit.orbits.FieldEquinoctialOrbit equinoctial orbit parameters} (a, e<sub>x</sub>,
  107.  *   e<sub>y</sub>, h<sub>x</sub>, h<sub>y</sub>, λ<sub>M</sub> or λ<sub>E</sub>
  108.  *   or λ<sub>v</sub>) in meters and radians,</li>
  109.  *   <li>the {@link org.orekit.orbits.FieldKeplerianOrbit Keplerian orbit parameters} (a, e, i, ω, Ω,
  110.  *   M or E or v) in meters and radians,</li>
  111.  *   <li>the {@link org.orekit.orbits.FieldCircularOrbit circular orbit parameters} (a, e<sub>x</sub>, e<sub>y</sub>, i,
  112.  *   Ω, α<sub>M</sub> or α<sub>E</sub> or α<sub>v</sub>) in meters
  113.  *   and radians,</li>
  114.  *   <li>the {@link org.orekit.orbits.FieldCartesianOrbit Cartesian orbit parameters} (x, y, z, v<sub>x</sub>,
  115.  *   v<sub>y</sub>, v<sub>z</sub>) in meters and meters per seconds.
  116.  * </ul>
  117.  * The last element is the mass in kilograms.
  118.  * <p>The following code snippet shows a typical setting for Low Earth Orbit propagation in
  119.  * equinoctial parameters and true longitude argument:</p>
  120.  * <pre>
  121.  * final T          zero      = field.getZero();
  122.  * final T          dP        = zero.add(0.001);
  123.  * final T          minStep   = zero.add(0.001);
  124.  * final T          maxStep   = zero.add(500);
  125.  * final T          initStep  = zero.add(60);
  126.  * final double[][] tolerance = FieldNumericalPropagator.tolerances(dP, orbit, OrbitType.EQUINOCTIAL);
  127.  * AdaptiveStepsizeFieldIntegrator&lt;T&gt; integrator = new DormandPrince853FieldIntegrator&lt;&gt;(field, minStep, maxStep, tolerance[0], tolerance[1]);
  128.  * integrator.setInitialStepSize(initStep);
  129.  * propagator = new FieldNumericalPropagator&lt;&gt;(field, integrator);
  130.  * </pre>
  131.  * <p>By default, at the end of the propagation, the propagator resets the initial state to the final state,
  132.  * thus allowing a new propagation to be started from there without recomputing the part already performed.
  133.  * This behaviour can be chenged by calling {@link #setResetAtEnd(boolean)}.
  134.  * </p>
  135.  * <p>Beware the same instance cannot be used simultaneously by different threads, the class is <em>not</em>
  136.  * thread-safe.</p>

  138.  * @see FieldSpacecraftState
  139.  * @see ForceModel
  140.  * @see org.orekit.propagation.sampling.FieldOrekitStepHandler
  141.  * @see org.orekit.propagation.sampling.FieldOrekitFixedStepHandler
  142.  * @see org.orekit.propagation.integration.FieldIntegratedEphemeris
  143.  * @see FieldTimeDerivativesEquations
  144.  *
  145.  * @author Mathieu Rom&eacute;ro
  146.  * @author Luc Maisonobe
  147.  * @author Guylaine Prat
  148.  * @author Fabien Maussion
  149.  * @author V&eacute;ronique Pommier-Maurussane
  150.  */
  151. public class FieldNumericalPropagator<T extends RealFieldElement<T>> extends FieldAbstractIntegratedPropagator<T> {

  153.     /** Force models used during the extrapolation of the FieldOrbit<T>, without Jacobians. */
  154.     private final List<ForceModel> forceModels;

  156.     /** Field used by this class.*/
  157.     private final Field<T> field;

  159.     /** boolean to ignore or not the creation of a NewtonianAttraction. */
  160.     private boolean ignoreCentralAttraction = false;

  162.     /** Create a new instance of NumericalPropagator, based on orbit definition mu.
  163.      * After creation, the instance is empty, i.e. the attitude provider is set to an
  164.      * unspecified default law and there are no perturbing forces at all.
  165.      * This means that if {@link #addForceModel addForceModel} is not
  166.      * called after creation, the integrated orbit will follow a Keplerian
  167.      * evolution only. The defaults are {@link OrbitType#EQUINOCTIAL}
  168.      * for {@link #setOrbitType(OrbitType) propagation
  169.      * orbit type} and {@link PositionAngle#TRUE} for {@link
  170.      * #setPositionAngleType(PositionAngle) position angle type}.
  171.      *
  172.      * <p>This constructor uses the {@link DataContext#getDefault() default data context}.
  173.      *
  174.      * @param integrator numerical integrator to use for propagation.
  175.      * @param field Field used by default
  176.      * @see #FieldNumericalPropagator(Field, FieldODEIntegrator, AttitudeProvider)
  177.      */
  178.     @DefaultDataContext
  179.     public FieldNumericalPropagator(final Field<T> field, final FieldODEIntegrator<T> integrator) {
  180.         this(field, integrator, Propagator.getDefaultLaw(DataContext.getDefault().getFrames()));
  181.     }

  183.     /** Create a new instance of NumericalPropagator, based on orbit definition mu.
  184.      * After creation, the instance is empty, i.e. the attitude provider is set to an
  185.      * unspecified default law and there are no perturbing forces at all.
  186.      * This means that if {@link #addForceModel addForceModel} is not
  187.      * called after creation, the integrated orbit will follow a Keplerian
  188.      * evolution only. The defaults are {@link OrbitType#EQUINOCTIAL}
  189.      * for {@link #setOrbitType(OrbitType) propagation
  190.      * orbit type} and {@link PositionAngle#TRUE} for {@link
  191.      * #setPositionAngleType(PositionAngle) position angle type}.
  192.      * @param field Field used by default
  193.      * @param integrator numerical integrator to use for propagation.
  194.      * @param attitudeProvider attitude law to use.
  195.      * @since 10.1
  196.      */
  197.     public FieldNumericalPropagator(final Field<T> field,
  198.                                     final FieldODEIntegrator<T> integrator,
  199.                                     final AttitudeProvider attitudeProvider) {
  200.         super(field, integrator, PropagationType.MEAN);
  201.         this.field = field;
  202.         forceModels = new ArrayList<ForceModel>();
  203.         initMapper(field);
  204.         setAttitudeProvider(attitudeProvider);
  205.         setMu(field.getZero().add(Double.NaN));
  206.         setSlaveMode();
  207.         setOrbitType(OrbitType.EQUINOCTIAL);
  208.         setPositionAngleType(PositionAngle.TRUE);
  209.     }

  211.     /** Set the flag to ignore or not the creation of a {@link NewtonianAttraction}.
  212.      * @param ignoreCentralAttraction if true, {@link NewtonianAttraction} is <em>not</em>
  213.      * added automatically if missing
  214.      */
  215.     public void setIgnoreCentralAttraction(final boolean ignoreCentralAttraction) {
  216.         this.ignoreCentralAttraction = ignoreCentralAttraction;
  217.     }

  219.      /** Set the central attraction coefficient μ.
  220.       * <p>
  221.       * Setting the central attraction coefficient is
  222.       * equivalent to {@link #addForceModel(ForceModel) add}
  223.       * a {@link NewtonianAttraction} force model.
  224.       * </p>
  225.      * @param mu central attraction coefficient (m³/s²)
  226.      * @see #addForceModel(ForceModel)
  227.      * @see #getAllForceModels()
  228.      */
  229.     public void setMu(final T mu) {
  230.         if (ignoreCentralAttraction) {
  231.             superSetMu(mu);
  232.         } else {
  233.             addForceModel(new NewtonianAttraction(mu.getReal()));
  234.         }
  235.     }

  237.     /** Set the central attraction coefficient μ only in upper class.
  238.      * @param mu central attraction coefficient (m³/s²)
  239.      */
  240.     private void superSetMu(final T mu) {
  241.         super.setMu(mu);
  242.     }

  244.     /** Check if Newtonian attraction force model is available.
  245.      * <p>
  246.      * Newtonian attraction is always the last force model in the list.
  247.      * </p>
  248.      * @return true if Newtonian attraction force model is available
  249.      */
  250.     private boolean hasNewtonianAttraction() {
  251.         final int last = forceModels.size() - 1;
  252.         return last >= 0 && forceModels.get(last) instanceof NewtonianAttraction;
  253.     }

  255.     /** Add a force model to the global perturbation model.
  256.      * <p>If this method is not called at all, the integrated orbit will follow
  257.      * a Keplerian evolution only.</p>
  258.      * @param model perturbing {@link ForceModel} to add
  259.      * @see #removeForceModels()
  260.      * @see #setMu(RealFieldElement)
  261.      */
  262.     public void addForceModel(final ForceModel model) {

  264.         if (model instanceof NewtonianAttraction) {
  265.             // we want to add the central attraction force model

  267.             try {
  268.                 // ensure we are notified of any mu change
  269.                 model.getParametersDrivers()[0].addObserver(new ParameterObserver() {
  270.                     /** {@inheritDoc} */
  271.                     @Override
  272.                     public void valueChanged(final double previousValue, final ParameterDriver driver) {
  273.                         superSetMu(field.getZero().add(driver.getValue()));
  274.                     }
  275.                 });
  276.             } catch (OrekitException oe) {
  277.                 // this should never happen
  278.                 throw new OrekitInternalError(oe);
  279.             }

  281.             if (hasNewtonianAttraction()) {
  282.                 // there is already a central attraction model, replace it
  283.                 forceModels.set(forceModels.size() - 1, model);
  284.             } else {
  285.                 // there are no central attraction model yet, add it at the end of the list
  286.                 forceModels.add(model);
  287.             }
  288.         } else {
  289.             // we want to add a perturbing force model
  290.             if (hasNewtonianAttraction()) {
  291.                 // insert the new force model before Newtonian attraction,
  292.                 // which should always be the last one in the list
  293.                 forceModels.add(forceModels.size() - 1, model);
  294.             } else {
  295.                 // we only have perturbing force models up to now, just append at the end of the list
  296.                 forceModels.add(model);
  297.             }
  298.         }

  300.     }

  302.     /** Remove all perturbing force models from the global perturbation model.
  303.      * <p>Once all perturbing forces have been removed (and as long as no new force
  304.      * model is added), the integrated orbit will follow a Keplerian evolution
  305.      * only.</p>
  306.      * @see #addForceModel(ForceModel)
  307.      */
  308.     public void removeForceModels() {
  309.         forceModels.clear();
  310.     }

  312.     /** Get all the force models, perturbing forces and Newtonian attraction included.
  313.      * @return list of perturbing force models, with Newtonian attraction being the
  314.      * last one
  315.      * @see #addForceModel(ForceModel)
  316.      * @see #setMu(RealFieldElement)
  317.      * @since 9.1
  318.      */
  319.     public List<ForceModel> getAllForceModels() {
  320.         return Collections.unmodifiableList(forceModels);
  321.     }

  323.     /** Set propagation orbit type.
  324.      * @param orbitType orbit type to use for propagation
  325.      */
  326.     public void setOrbitType(final OrbitType orbitType) {
  327.         super.setOrbitType(orbitType);
  328.     }

  330.     /** Get propagation parameter type.
  331.      * @return orbit type used for propagation
  332.      */
  333.     public OrbitType getOrbitType() {
  334.         return superGetOrbitType();
  335.     }

  337.     /** Get propagation parameter type.
  338.      * @return orbit type used for propagation
  339.      */
  340.     private OrbitType superGetOrbitType() {
  341.         return super.getOrbitType();
  342.     }

  344.     /** Set position angle type.
  345.      * <p>
  346.      * The position parameter type is meaningful only if {@link
  347.      * #getOrbitType() propagation orbit type}
  348.      * support it. As an example, it is not meaningful for propagation
  349.      * in {@link OrbitType#CARTESIAN Cartesian} parameters.
  350.      * </p>
  351.      * @param positionAngleType angle type to use for propagation
  352.      */
  353.     public void setPositionAngleType(final PositionAngle positionAngleType) {
  354.         super.setPositionAngleType(positionAngleType);
  355.     }

  357.     /** Get propagation parameter type.
  358.      * @return angle type to use for propagation
  359.      */
  360.     public PositionAngle getPositionAngleType() {
  361.         return super.getPositionAngleType();
  362.     }

  364.     /** Set the initial state.
  365.      * @param initialState initial state
  366.      */
  367.     public void setInitialState(final FieldSpacecraftState<T> initialState) {
  368.         resetInitialState(initialState);
  369.     }

  371.     /** {@inheritDoc} */
  372.     public void resetInitialState(final FieldSpacecraftState<T> state) {
  373.         super.resetInitialState(state);
  374.         if (!hasNewtonianAttraction()) {
  375.             setMu(state.getMu());
  376.         }
  377.         setStartDate(state.getDate());
  378.     }

  380.     /** {@inheritDoc} */
  381.     public TimeStampedFieldPVCoordinates<T> getPVCoordinates(final FieldAbsoluteDate<T> date, final Frame frame) {
  382.         return propagate(date).getPVCoordinates(frame);
  383.     }

  385.     /** {@inheritDoc} */
  386.     protected FieldStateMapper<T> createMapper(final FieldAbsoluteDate<T> referenceDate, final T mu,
  387.                                        final OrbitType orbitType, final PositionAngle positionAngleType,
  388.                                        final AttitudeProvider attitudeProvider, final Frame frame) {
  389.         return new FieldOsculatingMapper(referenceDate, mu, orbitType, positionAngleType, attitudeProvider, frame);
  390.     }

  392.     /** Internal mapper using directly osculating parameters. */
  393.     private class FieldOsculatingMapper extends FieldStateMapper<T> {

  395.         /** Simple constructor.
  396.          * <p>
  397.          * The position parameter type is meaningful only if {@link
  398.          * #getOrbitType() propagation orbit type}
  399.          * support it. As an example, it is not meaningful for propagation
  400.          * in {@link OrbitType#CARTESIAN Cartesian} parameters.
  401.          * </p>
  402.          * @param referenceDate reference date
  403.          * @param mu central attraction coefficient (m³/s²)
  404.          * @param orbitType orbit type to use for mapping
  405.          * @param positionAngleType angle type to use for propagation
  406.          * @param attitudeProvider attitude provider
  407.          * @param frame inertial frame
  408.          */
  409.         FieldOsculatingMapper(final FieldAbsoluteDate<T> referenceDate, final T mu,
  410.                               final OrbitType orbitType, final PositionAngle positionAngleType,
  411.                               final AttitudeProvider attitudeProvider, final Frame frame) {
  412.             super(referenceDate, mu, orbitType, positionAngleType, attitudeProvider, frame);
  413.         }

  415.         /** {@inheritDoc} */
  416.         public FieldSpacecraftState<T> mapArrayToState(final FieldAbsoluteDate<T> date, final T[] y, final T[] yDot,
  417.                                                        final PropagationType type) {
  418.             // the parameter type is ignored for the Numerical Propagator

  420.             final T mass = y[6];
  421.             if (mass.getReal() <= 0.0) {
  422.                 throw new OrekitException(OrekitMessages.SPACECRAFT_MASS_BECOMES_NEGATIVE, mass);
  423.             }

  425.             if (superGetOrbitType() == null) {
  426.                 // propagation uses absolute position-velocity-acceleration
  427.                 final FieldVector3D<T> p = new FieldVector3D<>(y[0],    y[1],    y[2]);
  428.                 final FieldVector3D<T> v = new FieldVector3D<>(y[3],    y[4],    y[5]);
  429.                 final FieldVector3D<T> a;
  430.                 final FieldAbsolutePVCoordinates<T> absPva;
  431.                 if (yDot == null) {
  432.                     absPva = new FieldAbsolutePVCoordinates<>(getFrame(), new TimeStampedFieldPVCoordinates<>(date, p, v, FieldVector3D.getZero(date.getField())));
  433.                 } else {
  434.                     a = new FieldVector3D<>(yDot[3], yDot[4], yDot[5]);
  435.                     absPva = new FieldAbsolutePVCoordinates<>(getFrame(), new TimeStampedFieldPVCoordinates<>(date, p, v, a));
  436.                 }

  438.                 final FieldAttitude<T> attitude = getAttitudeProvider().getAttitude(absPva, date, getFrame());
  439.                 return new FieldSpacecraftState<>(absPva, attitude, mass);
  440.             } else {
  441.                 // propagation uses regular orbits
  442.                 final FieldOrbit<T> orbit       = superGetOrbitType().mapArrayToOrbit(y, yDot, super.getPositionAngleType(), date, getMu(), getFrame());
  443.                 final FieldAttitude<T> attitude = getAttitudeProvider().getAttitude(orbit, date, getFrame());
  444.                 return new FieldSpacecraftState<>(orbit, attitude, mass);
  445.             }
  446.         }

  448.         /** {@inheritDoc} */
  449.         public void mapStateToArray(final FieldSpacecraftState<T> state, final T[] y, final T[] yDot) {
  450.             if (superGetOrbitType() == null) {
  451.                 // propagation uses absolute position-velocity-acceleration
  452.                 final FieldVector3D<T> p = state.getAbsPVA().getPosition();
  453.                 final FieldVector3D<T> v = state.getAbsPVA().getVelocity();
  454.                 y[0] = p.getX();
  455.                 y[1] = p.getY();
  456.                 y[2] = p.getZ();
  457.                 y[3] = v.getX();
  458.                 y[4] = v.getY();
  459.                 y[5] = v.getZ();
  460.                 y[6] = state.getMass();
  461.             }
  462.             else {
  463.                 superGetOrbitType().mapOrbitToArray(state.getOrbit(), super.getPositionAngleType(), y, yDot);
  464.                 y[6] = state.getMass();
  465.             }
  466.         }

  468.     }

  470.     /** {@inheritDoc} */
  471.     protected MainStateEquations<T> getMainStateEquations(final FieldODEIntegrator<T> integrator) {
  472.         return new Main(integrator);
  473.     }

  475.     /** Internal class for osculating parameters integration. */
  476.     private class Main implements MainStateEquations<T>, FieldTimeDerivativesEquations<T> {

  478.         /** Derivatives array. */
  479.         private final T[] yDot;

  481.         /** Current state. */
  482.         private FieldSpacecraftState<T> currentState;

  484.         /** Jacobian of the orbital parameters with respect to the Cartesian parameters. */
  485.         private T[][] jacobian;

  487.         /** Simple constructor.
  488.          * @param integrator numerical integrator to use for propagation.
  489.          */
  490.         Main(final FieldODEIntegrator<T> integrator) {

  492.             this.yDot     = MathArrays.buildArray(getField(),  7);
  493.             this.jacobian = MathArrays.buildArray(getField(),  6, 6);
  494.             for (final ForceModel forceModel : forceModels) {
  495.                 forceModel.getFieldEventsDetectors(getField()).forEach(detector -> setUpEventDetector(integrator, detector));
  496.             }

  498.             if (superGetOrbitType() == null) {
  499.                 // propagation uses absolute position-velocity-acceleration
  500.                 // we can set Jacobian once and for all
  501.                 for (int i = 0; i < jacobian.length; ++i) {
  502.                     Arrays.fill(jacobian[i], getField().getZero());
  503.                     jacobian[i][i] = getField().getOne();
  504.                 }
  505.             }

  507.         }

  509.         /** {@inheritDoc} */
  510.         @Override
  511.         public void init(final FieldSpacecraftState<T> initialState, final FieldAbsoluteDate<T> target) {
  512.             final SpacecraftState stateD  = initialState.toSpacecraftState();
  513.             final AbsoluteDate    targetD = target.toAbsoluteDate();
  514.             for (final ForceModel forceModel : forceModels) {
  515.                 forceModel.init(stateD, targetD);
  516.             }
  517.         }

  519.         /** {@inheritDoc} */
  520.         @Override
  521.         public T[] computeDerivatives(final FieldSpacecraftState<T> state) {
  522.             final T zero = state.getA().getField().getZero();
  523.             currentState = state;
  524.             Arrays.fill(yDot, zero);
  525.             if (superGetOrbitType() != null) {
  526.                 // propagation uses regular orbits
  527.                 currentState.getOrbit().getJacobianWrtCartesian(getPositionAngleType(), jacobian);
  528.             }

  530.             // compute the contributions of all perturbing forces,
  531.             // using the Kepler contribution at the end since
  532.             // NewtonianAttraction is always the last instance in the list
  533.             for (final ForceModel forceModel : forceModels) {
  534.                 forceModel.addContribution(state, this);
  535.             }

  537.             if (superGetOrbitType() == null) {
  538.                 // position derivative is velocity, and was not added above in the force models
  539.                 // (it is added when orbit type is non-null because NewtonianAttraction considers it)
  540.                 final FieldVector3D<T> velocity = currentState.getPVCoordinates().getVelocity();
  541.                 yDot[0] = yDot[0].add(velocity.getX());
  542.                 yDot[1] = yDot[1].add(velocity.getY());
  543.                 yDot[2] = yDot[2].add(velocity.getZ());
  544.             }

  546.             return yDot.clone();

  548.         }

  550.         /** {@inheritDoc} */
  551.         @Override
  552.         public void addKeplerContribution(final T mu) {
  553.             if (superGetOrbitType() == null) {

  555.                 // if mu is neither 0 nor NaN, we want to include Newtonian acceleration
  556.                 if (mu.getReal() > 0) {
  557.                     // velocity derivative is Newtonian acceleration
  558.                     final FieldVector3D<T> position = currentState.getPVCoordinates().getPosition();
  559.                     final double r2         = position.getNormSq().getReal();
  560.                     final double coeff      = -mu.getReal() / (r2 * FastMath.sqrt(r2));
  561.                     yDot[3] = yDot[3].add(coeff * position.getX().getReal());
  562.                     yDot[4] = yDot[4].add(coeff * position.getY().getReal());
  563.                     yDot[5] = yDot[5].add(coeff * position.getZ().getReal());
  564.                 }

  566.             } else {
  567.                 // propagation uses regular orbits
  568.                 currentState.getOrbit().addKeplerContribution(getPositionAngleType(), mu, yDot);
  569.             }
  570.         }

  572.         /** {@inheritDoc} */
  573.         @Override
  574.         public void addNonKeplerianAcceleration(final FieldVector3D<T> gamma) {
  575.             for (int i = 0; i < 6; ++i) {
  576.                 final T[] jRow = jacobian[i];
  577.                 yDot[i] = yDot[i].add(jRow[3].linearCombination(jRow[3], gamma.getX(),
  578.                                                                 jRow[4], gamma.getY(),
  579.                                                                 jRow[5], gamma.getZ()));
  580.             }
  581.         }

  583.         /** {@inheritDoc} */
  584.         @Override
  585.         public void addMassDerivative(final T q) {
  586.             if (q.getReal() > 0) {
  587.                 throw new OrekitIllegalArgumentException(OrekitMessages.POSITIVE_FLOW_RATE, q);
  588.             }
  589.             yDot[6] = yDot[6].add(q);
  590.         }

  592.     }

  594.     /** Estimate tolerance vectors for integrators.
  595.      * <p>
  596.      * The errors are estimated from partial derivatives properties of orbits,
  597.      * starting from a scalar position error specified by the user.
  598.      * Considering the energy conservation equation V = sqrt(mu (2/r - 1/a)),
  599.      * we get at constant energy (i.e. on a Keplerian trajectory):
  600.      * <pre>
  601.      * V² r |dV| = mu |dr|
  602.      * </pre>
  603.      * So we deduce a scalar velocity error consistent with the position error.
  604.      * From here, we apply orbits Jacobians matrices to get consistent errors
  605.      * on orbital parameters.
  606.      * <p>
  607.      * The tolerances are only <em>orders of magnitude</em>, and integrator tolerances
  608.      * are only local estimates, not global ones. So some care must be taken when using
  609.      * these tolerances. Setting 1mm as a position error does NOT mean the tolerances
  610.      * will guarantee a 1mm error position after several orbits integration.
  611.      * </p>
  612.      * @param dP user specified position error
  613.      * @param orbit reference orbit
  614.      * @param type propagation type for the meaning of the tolerance vectors elements
  615.      * (it may be different from {@code orbit.getType()})
  616.      * @return a two rows array, row 0 being the absolute tolerance error and row 1
  617.      * being the relative tolerance error
  618.      * @param <T> elements type
  619.      */
  620.     public static <T extends RealFieldElement<T>> double[][] tolerances(final T dP, final FieldOrbit<T> orbit, final OrbitType type) {

  622.         // estimate the scalar velocity error
  623.         final FieldPVCoordinates<T> pv = orbit.getPVCoordinates();
  624.         final T r2 = pv.getPosition().getNormSq();
  625.         final T v  = pv.getVelocity().getNorm();
  626.         final T dV = dP.multiply(orbit.getMu()).divide(v.multiply(r2));

  628.         return tolerances(dP, dV, orbit, type);

  630.     }

  632.     /** Estimate tolerance vectors for integrators when propagating in orbits.
  633.      * <p>
  634.      * The errors are estimated from partial derivatives properties of orbits,
  635.      * starting from scalar position and velocity errors specified by the user.
  636.      * <p>
  637.      * The tolerances are only <em>orders of magnitude</em>, and integrator tolerances
  638.      * are only local estimates, not global ones. So some care must be taken when using
  639.      * these tolerances. Setting 1mm as a position error does NOT mean the tolerances
  640.      * will guarantee a 1mm error position after several orbits integration.
  641.      * </p>
  642.      * @param <T> elements type
  643.      * @param dP user specified position error
  644.      * @param dV user specified velocity error
  645.      * @param orbit reference orbit
  646.      * @param type propagation type for the meaning of the tolerance vectors elements
  647.      * (it may be different from {@code orbit.getType()})
  648.      * @return a two rows array, row 0 being the absolute tolerance error and row 1
  649.      * being the relative tolerance error
  650.      * @since 10.3
  651.      */
  652.     public static <T extends RealFieldElement<T>> double[][] tolerances(final T dP, final T dV,
  653.                                                                         final FieldOrbit<T> orbit, final OrbitType type) {

  655.         final double[] absTol = new double[7];
  656.         final double[] relTol = new double[7];

  658.         // we set the mass tolerance arbitrarily to 1.0e-6 kg, as mass evolves linearly
  659.         // with trust, this often has no influence at all on propagation
  660.         absTol[6] = 1.0e-6;

  662.         if (type == OrbitType.CARTESIAN) {
  663.             absTol[0] = dP.getReal();
  664.             absTol[1] = dP.getReal();
  665.             absTol[2] = dP.getReal();
  666.             absTol[3] = dV.getReal();
  667.             absTol[4] = dV.getReal();
  668.             absTol[5] = dV.getReal();
  669.         } else {

  671.             // convert the orbit to the desired type
  672.             final T[][] jacobian = MathArrays.buildArray(dP.getField(), 6, 6);
  673.             final FieldOrbit<T> converted = type.convertType(orbit);
  674.             converted.getJacobianWrtCartesian(PositionAngle.TRUE, jacobian);

  676.             for (int i = 0; i < 6; ++i) {
  677.                 final  T[] row = jacobian[i];
  678.                 absTol[i] =     row[0].abs().multiply(dP).
  679.                             add(row[1].abs().multiply(dP)).
  680.                             add(row[2].abs().multiply(dP)).
  681.                             add(row[3].abs().multiply(dV)).
  682.                             add(row[4].abs().multiply(dV)).
  683.                             add(row[5].abs().multiply(dV)).
  684.                             getReal();
  685.                 if (Double.isNaN(absTol[i])) {
  686.                     throw new OrekitException(OrekitMessages.SINGULAR_JACOBIAN_FOR_ORBIT_TYPE, type);
  687.                 }
  688.             }

  690.         }

  692.         Arrays.fill(relTol, dP.divide(orbit.getPVCoordinates().getPosition().getNormSq().sqrt()).getReal());

  694.         return new double[][] { absTol, relTol };

  696.     }

  698. }
Created with JaCoCo 0.8.6.202009150832