AbstractFixedInitialCartesianSingleShooting.java

  1. /* Copyright 2022-2025 Romain Serra
  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.control.indirect.shooting;

  18. import org.hipparchus.CalculusFieldElement;
  19. import org.hipparchus.Field;
  20. import org.hipparchus.analysis.differentiation.Gradient;
  21. import org.hipparchus.analysis.differentiation.GradientField;
  22. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  23. import org.hipparchus.linear.LUDecomposition;
  24. import org.hipparchus.linear.MatrixUtils;
  25. import org.hipparchus.linear.RealMatrix;
  26. import org.hipparchus.ode.FieldOrdinaryDifferentialEquation;
  27. import org.hipparchus.ode.nonstiff.FieldExplicitRungeKuttaIntegrator;
  28. import org.hipparchus.util.FastMath;
  29. import org.hipparchus.util.MathArrays;
  30. import org.orekit.attitudes.FieldAttitude;
  31. import org.orekit.control.indirect.adjoint.CartesianAdjointDerivativesProvider;
  32. import org.orekit.control.indirect.adjoint.FieldCartesianAdjointDerivativesProvider;
  33. import org.orekit.control.indirect.adjoint.cost.ControlSwitchDetector;
  34. import org.orekit.control.indirect.adjoint.cost.FieldControlSwitchDetector;
  35. import org.orekit.control.indirect.shooting.propagation.AdjointDynamicsProvider;
  36. import org.orekit.control.indirect.shooting.propagation.ShootingPropagationSettings;
  37. import org.orekit.forces.ForceModel;
  38. import org.orekit.frames.Frame;
  39. import org.orekit.orbits.FieldCartesianOrbit;
  40. import org.orekit.propagation.FieldSpacecraftState;
  41. import org.orekit.propagation.SpacecraftState;
  42. import org.orekit.propagation.events.EventsLogger;
  43. import org.orekit.propagation.events.FieldEventDetector;
  44. import org.orekit.propagation.integration.FieldAdditionalDerivativesProvider;
  45. import org.orekit.propagation.integration.FieldCombinedDerivatives;
  46. import org.orekit.propagation.numerical.NumericalPropagator;
  47. import org.orekit.propagation.sampling.PropagationStepRecorder;
  48. import org.orekit.time.AbsoluteDate;
  49. import org.orekit.time.FieldAbsoluteDate;
  50. import org.orekit.utils.FieldAbsolutePVCoordinates;
  51. import org.orekit.utils.FieldPVCoordinates;

  52. import java.util.Arrays;
  53. import java.util.List;
  54. import java.util.stream.Collectors;

  55. /**
  56.  * Abstract class for indirect single shooting methods with fixed initial Cartesian state.
  57.  * Inheritors must implement the iteration update, assuming derivatives are needed.
  58.  *
  59.  * @author Romain Serra
  60.  * @since 13.0
  61.  * @see CartesianAdjointDerivativesProvider
  62.  * @see org.orekit.control.indirect.adjoint.FieldCartesianAdjointDerivativesProvider
  63.  */
  64. public abstract class AbstractFixedInitialCartesianSingleShooting extends AbstractIndirectShooting {

  65.     /** Template for initial state. Contains the initial Cartesian coordinates. */
  66.     private final SpacecraftState initialSpacecraftStateTemplate;

  67.     /**
  68.      * Step handler to record propagation steps.
  69.      */
  70.     private final PropagationStepRecorder propagationStepRecorder;

  71.     /** Event logger. */
  72.     private final EventsLogger eventsLogger;

  73.     /** Scales for automatic differentiation variables. */
  74.     private double[] scales;

  75.     /**
  76.      * Constructor with boundary conditions as orbits.
  77.      * @param propagationSettings propagation settings
  78.      * @param initialSpacecraftStateTemplate template for initial propagation state
  79.      */
  80.     protected AbstractFixedInitialCartesianSingleShooting(final ShootingPropagationSettings propagationSettings,
  81.                                                           final SpacecraftState initialSpacecraftStateTemplate) {
  82.         super(propagationSettings);
  83.         this.initialSpacecraftStateTemplate = initialSpacecraftStateTemplate;
  84.         this.propagationStepRecorder = new PropagationStepRecorder();
  85.         this.eventsLogger = new EventsLogger();
  86.     }

  87.     /**
  88.      * Build unit scales.
  89.      * @return scales
  90.      */
  91.     private double[] getUnitScales() {
  92.         final double[] unitScales = new double[getPropagationSettings().getAdjointDynamicsProvider().getDimension()];
  93.         Arrays.fill(unitScales, 1.);
  94.         return unitScales;
  95.     }

  96.     /**
  97.      * Protected getter for the differentiation scales.
  98.      * @return scales for variable scales
  99.      */
  100.     protected double[] getScales() {
  101.         return scales;
  102.     }

  103.     /**
  104.      * Returns the maximum number of iterations.
  105.      * @return maximum iterations
  106.      */
  107.     public abstract int getMaximumIterationCount();

  108.     /** {@inheritDoc} */
  109.     @Override
  110.     public ShootingBoundaryOutput solve(final double initialMass, final double[] initialGuess) {
  111.         return solve(initialMass, initialGuess, getUnitScales());
  112.     }

  113.     /**
  114.      * Solve for the boundary conditions, given an initial mass and an initial guess for the adjoint variables.
  115.      * Uses scales for automatic differentiation.
  116.      * @param initialMass initial mass
  117.      * @param initialGuess initial guess
  118.      * @param userScales scales
  119.      * @return boundary problem solution
  120.      */
  121.     public ShootingBoundaryOutput solve(final double initialMass, final double[] initialGuess,
  122.                                         final double[] userScales) {
  123.         scales = userScales.clone();
  124.         final SpacecraftState initialState = createStateWithMassAndAdjoint(initialMass, initialGuess);
  125.         final ShootingBoundaryOutput initialGuessSolution = computeCandidateSolution(initialState, 0);
  126.         if (initialGuessSolution.isConverged()) {
  127.             return initialGuessSolution;
  128.         } else {
  129.             return iterate(initialGuessSolution);
  130.         }
  131.     }

  132.     /** {@inheritDoc} */
  133.     @Override
  134.     protected NumericalPropagator buildPropagator(final SpacecraftState initialState) {
  135.         final NumericalPropagator propagator = super.buildPropagator(initialState);
  136.         propagator.setStepHandler(propagationStepRecorder);
  137.         final CartesianAdjointDerivativesProvider derivativesProvider = (CartesianAdjointDerivativesProvider)
  138.             getPropagationSettings().getAdjointDynamicsProvider().buildAdditionalDerivativesProvider();
  139.         eventsLogger.clearLoggedEvents();
  140.         derivativesProvider.getCost().getEventDetectors()
  141.                 .forEach(detector -> propagator.addEventDetector(eventsLogger.monitorDetector(detector)));
  142.         return propagator;
  143.     }

  144.     /**
  145.      * Form solution container with input initial state.
  146.      * @param initialState initial state
  147.      * @param iterationCount iteration count
  148.      * @return candidate solution
  149.      */
  150.     public abstract ShootingBoundaryOutput computeCandidateSolution(SpacecraftState initialState, int iterationCount);

  151.     /**
  152.      * Iterate on initial guess to solve boundary problem.
  153.      * @param initialGuess initial guess
  154.      * @return solution (or null pointer if not converged)
  155.      */
  156.     private ShootingBoundaryOutput iterate(final ShootingBoundaryOutput initialGuess) {
  157.         int iterationCount = 1;
  158.         SpacecraftState initialState = initialGuess.getInitialState();
  159.         ShootingBoundaryOutput candidateSolution = initialGuess;
  160.         final String adjointName = getPropagationSettings().getAdjointDynamicsProvider().getAdjointName();
  161.         double[] initialAdjoint = initialState.getAdditionalState(adjointName).clone();
  162.         final double initialMass = initialState.getMass();
  163.         while (iterationCount < getMaximumIterationCount()) {
  164.             if (candidateSolution.isConverged()) {
  165.                 return candidateSolution;
  166.             }
  167.             final FieldSpacecraftState<Gradient> fieldInitialState = createFieldInitialStateWithMassAndAdjoint(initialMass,
  168.                 initialAdjoint);
  169.             initialState = fieldInitialState.toSpacecraftState();
  170.             final FieldSpacecraftState<Gradient> fieldTerminalState = propagateField(fieldInitialState);
  171.             initialAdjoint = updateShootingVariables(initialAdjoint, fieldTerminalState);
  172.             if (Double.isNaN(initialAdjoint[0])) {
  173.                 return candidateSolution;
  174.             } else {
  175.                 candidateSolution = computeCandidateSolution(initialState, iterationCount);
  176.             }
  177.             iterationCount++;
  178.         }
  179.         return candidateSolution;
  180.     }

  181.     /**
  182.      * Propagate in Field. Does not use a full propagator (only the integrator, moving step by step using the history of non-Field propagation).
  183.      * It is faster as adaptive step and event detection are particularly slow with Field.
  184.      * @param fieldInitialState initial state
  185.      * @return terminal state
  186.      */
  187.     private FieldSpacecraftState<Gradient> propagateField(final FieldSpacecraftState<Gradient> fieldInitialState) {
  188.         final FieldAbsoluteDate<Gradient> initialDate = fieldInitialState.getDate();
  189.         final FieldExplicitRungeKuttaIntegrator<Gradient> fieldIntegrator = buildFieldIntegrator(fieldInitialState);
  190.         final FieldOrdinaryDifferentialEquation<Gradient> fieldODE = buildFieldODE(fieldInitialState.getDate());
  191.         final AdjointDynamicsProvider dynamicsProvider = getPropagationSettings().getAdjointDynamicsProvider();
  192.         AbsoluteDate date = initialDate.toAbsoluteDate();
  193.         Gradient[] integrationState = formatToArray(fieldInitialState, dynamicsProvider);
  194.         // step-by-step integration
  195.         final List<EventsLogger.LoggedEvent> loggedEvents = eventsLogger.getLoggedEvents();
  196.         final List<AbsoluteDate> stepDates = propagationStepRecorder.copyStates().stream().map(SpacecraftState::getDate)
  197.                 .collect(Collectors.toList());
  198.         for (final AbsoluteDate stepDate: stepDates) {
  199.             final Gradient time = initialDate.durationFrom(date).negate();
  200.             final Gradient nextTime = initialDate.durationFrom(stepDate).negate();
  201.             integrationState = fieldIntegrator.singleStep(fieldODE, time, integrationState, nextTime);
  202.             // deal with switch event if any
  203.             if (!loggedEvents.isEmpty()) {
  204.                 for (final EventsLogger.LoggedEvent loggedEvent: loggedEvents) {
  205.                     final SpacecraftState loggedState = loggedEvent.getState();
  206.                     if (loggedEvent.getEventDetector() instanceof ControlSwitchDetector && loggedState.getDate().isEqualTo(stepDate)) {
  207.                         final ControlSwitchDetector switchDetector = (ControlSwitchDetector) loggedEvent.getEventDetector();
  208.                         integrationState = findSwitchEventAndUpdateState(date, integrationState,
  209.                                 initialDate.toAbsoluteDate(), switchDetector, dynamicsProvider);
  210.                     }
  211.                 }
  212.             }
  213.             date = new AbsoluteDate(stepDate);
  214.         }
  215.         return formatFromArray(date, integrationState);
  216.     }

  217.     /**
  218.      * Create initial state with input mass.
  219.      * @param initialMass initial mass
  220.      * @return state
  221.      */
  222.     protected SpacecraftState createInitialStateWithMass(final double initialMass) {
  223.         return initialSpacecraftStateTemplate.withMass(initialMass);
  224.     }

  225.     /**
  226.      * Create initial state with input mass and adjoint vector.
  227.      * @param initialMass initial mass
  228.      * @param initialAdjoint initial adjoint variables
  229.      * @return state
  230.      */
  231.     private SpacecraftState createStateWithMassAndAdjoint(final double initialMass, final double[] initialAdjoint) {
  232.         final String adjointName = getPropagationSettings().getAdjointDynamicsProvider().getAdjointName();
  233.         return createInitialStateWithMass(initialMass).addAdditionalData(adjointName, initialAdjoint);
  234.     }

  235.     /**
  236.      * Create initial Gradient state with input mass and adjoint vector, the latter being the independent variables.
  237.      * @param initialMass initial mass
  238.      * @param initialAdjoint initial adjoint variables
  239.      * @return state
  240.      */
  241.     protected FieldSpacecraftState<Gradient> createFieldInitialStateWithMassAndAdjoint(final double initialMass,
  242.                                                                                        final double[] initialAdjoint) {
  243.         final int parametersNumber = initialAdjoint.length;
  244.         final GradientField field = GradientField.getField(parametersNumber);
  245.         final FieldSpacecraftState<Gradient> stateWithoutAdjoint = new FieldSpacecraftState<>(field,
  246.                 createInitialStateWithMass(initialMass));
  247.         final Gradient[] fieldInitialAdjoint = MathArrays.buildArray(field, initialAdjoint.length);
  248.         for (int i = 0; i < parametersNumber; i++) {
  249.             fieldInitialAdjoint[i] = Gradient.variable(parametersNumber, i, 0.).multiply(getScales()[i]).add(initialAdjoint[i]);
  250.         }
  251.         final String adjointName = getPropagationSettings().getAdjointDynamicsProvider().getAdjointName();
  252.         return stateWithoutAdjoint.addAdditionalData(adjointName, fieldInitialAdjoint);
  253.     }

  254.     /**
  255.      * Create state.
  256.      * @param date epoch
  257.      * @param cartesian Cartesian variables
  258.      * @param mass mass
  259.      * @param adjoint adjoint variables
  260.      * @param <T> field type
  261.      * @return state
  262.      */
  263.     protected <T extends CalculusFieldElement<T>> FieldSpacecraftState<T> createFieldState(final FieldAbsoluteDate<T> date,
  264.                                                                                            final T[] cartesian,
  265.                                                                                            final T mass,
  266.                                                                                            final T[] adjoint) {
  267.         final Field<T> field = mass.getField();
  268.         final Frame frame = getPropagationSettings().getPropagationFrame();
  269.         final FieldVector3D<T> position = new FieldVector3D<>(Arrays.copyOfRange(cartesian, 0, 3));
  270.         final FieldVector3D<T> velocity = new FieldVector3D<>(Arrays.copyOfRange(cartesian, 3, 6));
  271.         final FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>(position, velocity);
  272.         final FieldSpacecraftState<T> stateWithoutAdjoint;
  273.         if (initialSpacecraftStateTemplate.isOrbitDefined()) {
  274.             final T mu = field.getZero().newInstance(initialSpacecraftStateTemplate.getOrbit().getMu());
  275.             final FieldCartesianOrbit<T> orbit = new FieldCartesianOrbit<>(pvCoordinates, frame, date, mu);
  276.             final FieldAttitude<T> attitude = getPropagationSettings().getAttitudeProvider().getAttitude(orbit,
  277.                     orbit.getDate(), orbit.getFrame());
  278.             stateWithoutAdjoint = new FieldSpacecraftState<>(orbit, attitude).withMass(mass);
  279.         } else {
  280.             final FieldAbsolutePVCoordinates<T> absolutePVCoordinates = new FieldAbsolutePVCoordinates<>(frame, date,
  281.                     pvCoordinates);
  282.             final FieldAttitude<T> attitude = getPropagationSettings().getAttitudeProvider().getAttitude(absolutePVCoordinates,
  283.                     absolutePVCoordinates.getDate(), absolutePVCoordinates.getFrame());
  284.             stateWithoutAdjoint = new FieldSpacecraftState<>(absolutePVCoordinates, attitude).withMass(mass);
  285.         }
  286.         final String adjointName = getPropagationSettings().getAdjointDynamicsProvider().getAdjointName();
  287.         return stateWithoutAdjoint.addAdditionalData(adjointName, adjoint);
  288.     }

  289.     /**
  290.      * Update shooting variables.
  291.      * @param originalShootingVariables original shooting variables (before update)
  292.      * @param fieldTerminalState final state of gradient propagation
  293.      * @return updated shooting variables
  294.      */
  295.     protected abstract double[] updateShootingVariables(double[] originalShootingVariables,
  296.                                                         FieldSpacecraftState<Gradient> fieldTerminalState);

  297.     /**
  298.      * Build Ordinary Differential Equation for Field.
  299.      * @param referenceDate date defining the origin of times
  300.      * @param <T> field type
  301.      * @return Field ODE
  302.      * @since 13.0
  303.      */
  304.     protected <T extends CalculusFieldElement<T>> FieldOrdinaryDifferentialEquation<T> buildFieldODE(final FieldAbsoluteDate<T> referenceDate) {
  305.         final Field<T> field = referenceDate.getField();
  306.         final AdjointDynamicsProvider adjointDynamicsProvider = getPropagationSettings().getAdjointDynamicsProvider();
  307.         final FieldAdditionalDerivativesProvider<T> derivativesProvider = adjointDynamicsProvider
  308.                 .buildFieldAdditionalDerivativesProvider(field);

  309.         return new FieldOrdinaryDifferentialEquation<T>() {
  310.             @Override
  311.             public int getDimension() {
  312.                 return 7 + adjointDynamicsProvider.getDimension();
  313.             }

  314.             @Override
  315.             public T[] computeDerivatives(final T t, final T[] y) {
  316.                 // build state
  317.                 final T[] cartesian = Arrays.copyOfRange(y, 0, 6);
  318.                 final FieldAbsoluteDate<T> date = referenceDate.shiftedBy(t);
  319.                 final Field<T> field = date.getField();
  320.                 final T zero = field.getZero();
  321.                 final T[] adjoint = Arrays.copyOfRange(y, 7, y.length);
  322.                 final FieldSpacecraftState<T> state = createFieldState(date, cartesian, y[6], adjoint);
  323.                 // compute derivatives
  324.                 final T[] yDot = MathArrays.buildArray(field, getDimension());
  325.                 yDot[0] = zero.add(y[3]);
  326.                 yDot[1] = zero.add(y[4]);
  327.                 yDot[2] = zero.add(y[5]);
  328.                 FieldVector3D<T> totalAcceleration = FieldVector3D.getZero(field);
  329.                 for (final ForceModel forceModel: getPropagationSettings().getForceModels()) {
  330.                     final FieldVector3D<T> acceleration = forceModel.acceleration(state, forceModel.getParameters(field));
  331.                     totalAcceleration = totalAcceleration.add(acceleration);
  332.                 }
  333.                 yDot[3] = totalAcceleration.getX();
  334.                 yDot[4] = totalAcceleration.getY();
  335.                 yDot[5] = totalAcceleration.getZ();
  336.                 final FieldCombinedDerivatives<T> combinedDerivatives = derivativesProvider.combinedDerivatives(state);
  337.                 final T[] cartesianDotContribution = combinedDerivatives.getMainStateDerivativesIncrements();
  338.                 for (int i = 0; i < cartesianDotContribution.length; i++) {
  339.                     yDot[i] = yDot[i].add(cartesianDotContribution[i]);
  340.                 }
  341.                 final T[] adjointDot = combinedDerivatives.getAdditionalDerivatives();
  342.                 System.arraycopy(adjointDot, 0, yDot, yDot.length - adjointDot.length, adjointDot.length);
  343.                 return yDot;
  344.             }
  345.         };
  346.     }

  347.     /**
  348.      * Form array from Orekit object.
  349.      * @param fieldState state
  350.      * @param dynamicsProvider adjoint dynamics provider
  351.      * @return propagation state as array
  352.      */
  353.     private Gradient[] formatToArray(final FieldSpacecraftState<Gradient> fieldState,
  354.                                      final AdjointDynamicsProvider dynamicsProvider) {
  355.         final Gradient[] integrationState = MathArrays.buildArray(fieldState.getMass().getField(),
  356.                 7 + dynamicsProvider.getDimension());
  357.         final FieldPVCoordinates<Gradient> pvCoordinates = fieldState.getPVCoordinates();
  358.         System.arraycopy(pvCoordinates.getPosition().toArray(), 0, integrationState, 0, 3);
  359.         System.arraycopy(pvCoordinates.getVelocity().toArray(), 0, integrationState, 3, 3);
  360.         integrationState[6] = fieldState.getMass();
  361.         System.arraycopy(fieldState.getAdditionalData(dynamicsProvider.getAdjointName()), 0, integrationState,
  362.                 7, dynamicsProvider.getDimension());
  363.         return integrationState;
  364.     }

  365.     /**
  366.      * Form Orekit object from array.
  367.      * @param date date
  368.      * @param integrationState propagation state as array
  369.      * @return Orekit state
  370.      */
  371.     private FieldSpacecraftState<Gradient> formatFromArray(final AbsoluteDate date, final Gradient[] integrationState) {
  372.         final Gradient[] terminalCartesian = Arrays.copyOfRange(integrationState, 0, 6);
  373.         final Gradient[] terminalAdjoint = Arrays.copyOfRange(integrationState, 7, integrationState.length);
  374.         final Field<Gradient> field = terminalCartesian[0].getField();
  375.         return createFieldState(new FieldAbsoluteDate<>(field, date), terminalCartesian, integrationState[6],
  376.                 terminalAdjoint);
  377.     }

  378.     /**
  379.      * Iterate over field switch detectors and find the one that was triggered in the non-Field propagation.
  380.      * Then, use it to update the gradient.
  381.      * @param date date
  382.      * @param integrationState integration variables
  383.      * @param referenceDate date at start of propagation
  384.      * @param switchDetector switch detector
  385.      * @param dynamicsProvider adjoint dynamics provider
  386.      * @return update integration state
  387.      */
  388.     private Gradient[] findSwitchEventAndUpdateState(final AbsoluteDate date, final Gradient[] integrationState,
  389.                                                      final AbsoluteDate referenceDate,
  390.                                                      final ControlSwitchDetector switchDetector,
  391.                                                      final AdjointDynamicsProvider dynamicsProvider) {
  392.         final int shootingVariables = dynamicsProvider.getDimension();
  393.         final int increasedVariables = shootingVariables + 1;
  394.         final GradientField increasedVariablesField = GradientField.getField(increasedVariables);
  395.         final FieldCartesianAdjointDerivativesProvider<Gradient> fieldDerivativesProvider =
  396.                 (FieldCartesianAdjointDerivativesProvider<Gradient>) dynamicsProvider.buildFieldAdditionalDerivativesProvider(increasedVariablesField);
  397.         final List<FieldEventDetector<Gradient>> fieldEventDetectors = fieldDerivativesProvider.getCost()
  398.                 .getFieldEventDetectors(increasedVariablesField).collect(Collectors.toList());
  399.         final FieldSpacecraftState<Gradient> fieldState = formatFromArray(date, integrationState);
  400.         final double expectedG = switchDetector.g(fieldState.toSpacecraftState());
  401.         for (final FieldEventDetector<Gradient> fieldEventDetector : fieldEventDetectors) {
  402.             if (fieldEventDetector instanceof FieldControlSwitchDetector) {
  403.                 final double actualG = fieldEventDetector.g(fieldState).getReal();
  404.                 if (FastMath.abs(actualG - expectedG) < 1e-10) {
  405.                     return updateStateWithTaylorMapInversion(date, integrationState, referenceDate,
  406.                             (FieldControlSwitchDetector<Gradient>) fieldEventDetector, dynamicsProvider.getAdjointName());
  407.                 }
  408.             }
  409.         }
  410.         return integrationState;
  411.     }

  412.     /**
  413.      * Update the differential information by taking into account that a state-dependent event was triggered.
  414.      * @param date date
  415.      * @param integrationState integration variables
  416.      * @param initialDate date at start of propagation
  417.      * @param fieldDetector switch detector
  418.      * @param adjointName adjoint name
  419.      * @return updated integration state
  420.      */
  421.     private Gradient[] updateStateWithTaylorMapInversion(final AbsoluteDate date, final Gradient[] integrationState,
  422.                                                          final AbsoluteDate initialDate,
  423.                                                          final FieldControlSwitchDetector<Gradient> fieldDetector,
  424.                                                          final String adjointName) {
  425.         // form array with increased gradient size
  426.         final int increasedVariables = integrationState[0].getFreeParameters() + 1;
  427.         final GradientField increasedVariablesField = GradientField.getField(increasedVariables);
  428.         final Gradient[] increasedVariablesArray = MathArrays.buildArray(increasedVariablesField,
  429.                 integrationState.length);
  430.         for (int i = 0; i < integrationState.length; i++) {
  431.             increasedVariablesArray[i] = integrationState[i].stackVariable();
  432.         }
  433.         // evaluate event function in Taylor algebra with time as additional gradient variable
  434.         final Gradient dt = Gradient.variable(increasedVariables, increasedVariables - 1, 1);
  435.         final FieldAbsoluteDate<Gradient> referenceDate = new FieldAbsoluteDate<>(increasedVariablesField, initialDate);
  436.         final FieldOrdinaryDifferentialEquation<Gradient> fieldODE = buildFieldODE(referenceDate);
  437.         final Gradient[] derivatives = fieldODE.computeDerivatives(dt.add(date.durationFrom(initialDate)),
  438.                 increasedVariablesArray);
  439.         final FieldSpacecraftState<Gradient> fieldState = formatFromArray(date, increasedVariablesArray);
  440.         final FieldSpacecraftState<Gradient> fieldStateWithAdjointDerivative = fieldState.addAdditionalStateDerivative(adjointName,
  441.                 Arrays.copyOfRange(derivatives, derivatives.length - 7, derivatives.length));
  442.         final Gradient g = fieldDetector.g(fieldStateWithAdjointDerivative.shiftedBy(dt));
  443.         // invert map
  444.         return invertTaylorMap(increasedVariablesArray, g);
  445.     }

  446.     /**
  447.      * Invert so-called Taylor map to make the event function value an independent variable.
  448.      * Then nullify its variation to keep only the derivatives of interest.
  449.      * @param state integration variables with dt as last gradient variable
  450.      * @param g event function evaluated with dt as last gradient variable
  451.      * @return update integration variables
  452.      */
  453.     private static Gradient[] invertTaylorMap(final Gradient[] state, final Gradient g) {
  454.         // swap dt and g as variables of algebra
  455.         final int increasedGradientDimension = g.getFreeParameters();
  456.         final RealMatrix rightMatrix = MatrixUtils.createRealIdentityMatrix(increasedGradientDimension);
  457.         rightMatrix.setRow(rightMatrix.getRowDimension() - 1, g.getGradient());
  458.         final LUDecomposition luDecomposition = new LUDecomposition(rightMatrix, 0.);
  459.         final RealMatrix inverted = luDecomposition.getSolver().getInverse();
  460.         final double[][] leftMatrixCoefficients = new double[state.length + 1][];
  461.         for (int i = 0; i < state.length; i++) {
  462.             leftMatrixCoefficients[i] = state[i].getGradient();
  463.         }
  464.         leftMatrixCoefficients[leftMatrixCoefficients.length - 1] = g.getGradient();
  465.         final RealMatrix product = MatrixUtils.createRealMatrix(leftMatrixCoefficients).multiply(inverted);
  466.         // pack into array with original gradient dimension
  467.         final int gradientDimension = increasedGradientDimension - 1;
  468.         final GradientField field = GradientField.getField(gradientDimension);
  469.         final Gradient[] outputState = MathArrays.buildArray(field, state.length);
  470.         for (int i = 0; i < outputState.length; i++) {
  471.             final double[] gradient = new double[gradientDimension];
  472.             System.arraycopy(product.getRow(i), 0, gradient, 0, gradientDimension);
  473.             outputState[i] = new Gradient(state[i].getValue(), gradient);
  474.         }
  475.         return outputState;
  476.     }

  477. }