EpochDerivativesEquations.java

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

  18. import java.util.IdentityHashMap;
  19. import java.util.Map;

  20. import org.hipparchus.analysis.differentiation.Gradient;
  21. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  22. import org.hipparchus.linear.Array2DRowRealMatrix;
  23. import org.hipparchus.linear.DecompositionSolver;
  24. import org.hipparchus.linear.MatrixUtils;
  25. import org.hipparchus.linear.QRDecomposition;
  26. import org.hipparchus.linear.RealMatrix;
  27. import org.orekit.errors.OrekitException;
  28. import org.orekit.errors.OrekitMessages;
  29. import org.orekit.forces.ForceModel;
  30. import org.orekit.forces.gravity.ThirdBodyAttractionEpoch;
  31. import org.orekit.propagation.FieldSpacecraftState;
  32. import org.orekit.propagation.SpacecraftState;
  33. import org.orekit.propagation.integration.AdditionalDerivativesProvider;
  34. import org.orekit.propagation.integration.CombinedDerivatives;
  35. import org.orekit.utils.ParameterDriver;
  36. import org.orekit.utils.ParameterDriversList;
  37. import org.orekit.utils.TimeSpanMap.Span;

  38. /** Computes derivatives of the acceleration, including ThirdBodyAttraction.
  39.  *
  40.  * {@link AdditionalDerivativesProvider Provider} computing the partial derivatives
  41.  * of the state (orbit) with respect to initial state and force models parameters.
  42.  * <p>
  43.  * This set of equations are automatically added to a {@link NumericalPropagator numerical propagator}
  44.  * in order to compute partial derivatives of the orbit along with the orbit itself. This is
  45.  * useful for example in orbit determination applications.
  46.  * </p>
  47.  * <p>
  48.  * The partial derivatives with respect to initial state can be either dimension 6
  49.  * (orbit only) or 7 (orbit and mass).
  50.  * </p>
  51.  * <p>
  52.  * The partial derivatives with respect to force models parameters has a dimension
  53.  * equal to the number of selected parameters. Parameters selection is implemented at
  54.  * {@link ForceModel force models} level. Users must retrieve a {@link ParameterDriver
  55.  * parameter driver} using {@link ForceModel#getParameterDriver(String)} and then
  56.  * select it by calling {@link ParameterDriver#setSelected(boolean) setSelected(true)}.
  57.  * </p>
  58.  * <p>
  59.  * If several force models provide different {@link ParameterDriver drivers} for the
  60.  * same parameter name, selecting any of these drivers has the side effect of
  61.  * selecting all the drivers for this shared parameter. In this case, the partial
  62.  * derivatives will be the sum of the partial derivatives contributed by the
  63.  * corresponding force models. This case typically arises for central attraction
  64.  * coefficient, which has an influence on {@link org.orekit.forces.gravity.NewtonianAttraction
  65.  * Newtonian attraction}, {@link org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel
  66.  * gravity field}, and {@link org.orekit.forces.gravity.Relativity relativity}.
  67.  * </p>
  68.  * @author V&eacute;ronique Pommier-Maurussane
  69.  * @author Luc Maisonobe
  70.  * @since 10.2
  71.  */
  72. public class EpochDerivativesEquations
  73.     implements AdditionalDerivativesProvider  {

  74.     /** State dimension, fixed to 6. */
  75.     public static final int STATE_DIMENSION = 6;

  76.     /** Propagator computing state evolution. */
  77.     private final NumericalPropagator propagator;

  78.     /** Selected parameters for Jacobian computation. */
  79.     private ParameterDriversList selected;

  80.     /** Parameters map. */
  81.     private Map<String, Integer> map;

  82.     /** Name. */
  83.     private final String name;

  84.     /** Simple constructor.
  85.      * <p>
  86.      * Upon construction, this set of equations is <em>automatically</em> added to
  87.      * the propagator by calling its {@link
  88.      * NumericalPropagator#addAdditionalDerivativesProvider(AdditionalDerivativesProvider)} method. So
  89.      * there is no need to call this method explicitly for these equations.
  90.      * </p>
  91.      * @param name name of the partial derivatives equations
  92.      * @param propagator the propagator that will handle the orbit propagation
  93.      */
  94.     public EpochDerivativesEquations(final String name, final NumericalPropagator propagator) {
  95.         this.name                   = name;
  96.         this.selected               = null;
  97.         this.map                    = null;
  98.         this.propagator             = propagator;
  99.         propagator.addAdditionalDerivativesProvider(this);
  100.     }

  101.     /** {@inheritDoc} */
  102.     public String getName() {
  103.         return name;
  104.     }

  105.     /** {@inheritDoc} */
  106.     @Override
  107.     public int getDimension() {
  108.         freezeParametersSelection();
  109.         return 6 * (6 + selected.getNbParams() + 1);
  110.     }

  111.     /** Freeze the selected parameters from the force models.
  112.      */
  113.     private void freezeParametersSelection() {
  114.         if (selected == null) {

  115.             // first pass: gather all parameters, binding similar names together
  116.             selected = new ParameterDriversList();
  117.             for (final ForceModel provider : propagator.getAllForceModels()) {
  118.                 for (final ParameterDriver driver : provider.getParametersDrivers()) {
  119.                     selected.add(driver);
  120.                 }
  121.             }

  122.             // second pass: now that shared parameter names are bound together,
  123.             // their selections status have been synchronized, we can filter them
  124.             selected.filter(true);

  125.             // third pass: sort parameters lexicographically
  126.             selected.sort();

  127.             // fourth pass: set up a map between parameters drivers and matrices columns
  128.             map = new IdentityHashMap<>();
  129.             int parameterIndex = 0;
  130.             int previousParameterIndex = 0;
  131.             for (final ParameterDriver selectedDriver : selected.getDrivers()) {
  132.                 for (final ForceModel provider : propagator.getAllForceModels()) {
  133.                     for (final ParameterDriver driver : provider.getParametersDrivers()) {
  134.                         if (driver.getName().equals(selectedDriver.getName())) {
  135.                             previousParameterIndex = parameterIndex;
  136.                             for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
  137.                                 map.put(span.getData(), previousParameterIndex++);
  138.                             }
  139.                         }
  140.                     }
  141.                 }
  142.                 parameterIndex = previousParameterIndex;
  143.             }

  144.         }
  145.     }

  146.     /** Set the initial value of the Jacobian with respect to state and parameter.
  147.      * <p>
  148.      * This method is equivalent to call {@link #setInitialJacobians(SpacecraftState,
  149.      * double[][], double[][])} with dYdY0 set to the identity matrix and dYdP set
  150.      * to a zero matrix.
  151.      * </p>
  152.      * <p>
  153.      * The force models parameters for which partial derivatives are desired,
  154.      * <em>must</em> have been {@link ParameterDriver#setSelected(boolean) selected}
  155.      * before this method is called, so proper matrices dimensions are used.
  156.      * </p>
  157.      * @param s0 initial state
  158.      * @return state with initial Jacobians added
  159.      */
  160.     public SpacecraftState setInitialJacobians(final SpacecraftState s0) {
  161.         freezeParametersSelection();
  162.         final int epochStateDimension = 6;
  163.         final double[][] dYdY0 = new double[epochStateDimension][epochStateDimension];
  164.         final double[][] dYdP  = new double[epochStateDimension][selected.getNbValuesToEstimate() + 6];
  165.         for (int i = 0; i < epochStateDimension; ++i) {
  166.             dYdY0[i][i] = 1.0;
  167.         }
  168.         return setInitialJacobians(s0, dYdY0, dYdP);
  169.     }

  170.     /** Set the initial value of the Jacobian with respect to state and parameter.
  171.      * <p>
  172.      * The returned state must be added to the propagator (it is not done
  173.      * automatically, as the user may need to add more states to it).
  174.      * </p>
  175.      * <p>
  176.      * The force models parameters for which partial derivatives are desired,
  177.      * <em>must</em> have been {@link ParameterDriver#setSelected(boolean) selected}
  178.      * before this method is called, and the {@code dY1dP} matrix dimension <em>must</em>
  179.      * be consistent with the selection.
  180.      * </p>
  181.      * @param s1 current state
  182.      * @param dY1dY0 Jacobian of current state at time t₁ with respect
  183.      * to state at some previous time t₀ (must be 6x6)
  184.      * @param dY1dP Jacobian of current state at time t₁ with respect
  185.      * to parameters (may be null if no parameters are selected)
  186.      * @return state with initial Jacobians added
  187.      */
  188.     public SpacecraftState setInitialJacobians(final SpacecraftState s1,
  189.                                                final double[][] dY1dY0, final double[][] dY1dP) {

  190.         freezeParametersSelection();

  191.         // Check dimensions
  192.         final int stateDimEpoch = dY1dY0.length;
  193.         if (stateDimEpoch != 6 || stateDimEpoch != dY1dY0[0].length) {
  194.             throw new OrekitException(OrekitMessages.STATE_JACOBIAN_NOT_6X6,
  195.                                       stateDimEpoch, dY1dY0[0].length);
  196.         }
  197.         if (dY1dP != null && stateDimEpoch != dY1dP.length) {
  198.             throw new OrekitException(OrekitMessages.STATE_AND_PARAMETERS_JACOBIANS_ROWS_MISMATCH,
  199.                                       stateDimEpoch, dY1dP.length);
  200.         }

  201.         // store the matrices as a single dimension array
  202.         final double[] p = new double[STATE_DIMENSION * (STATE_DIMENSION + selected.getNbValuesToEstimate()) + 6];
  203.         setInitialJacobians(s1, dY1dY0, dY1dP, p);

  204.         // set value in propagator
  205.         return s1.addAdditionalState(name, p);

  206.     }

  207.     /** Set the Jacobian with respect to state into a one-dimensional additional state array.
  208.      * <p>
  209.      * This method converts the Jacobians to Cartesian parameters and put the converted data
  210.      * in the one-dimensional {@code p} array.
  211.      * </p>
  212.      * @param state spacecraft state
  213.      * @param dY1dY0 Jacobian of current state at time t₁
  214.      * with respect to state at some previous time t₀
  215.      * @param dY1dP Jacobian of current state at time t₁
  216.      * with respect to parameters (may be null if there are no parameters)
  217.      * @param p placeholder where to put the one-dimensional additional state
  218.      */
  219.     public void setInitialJacobians(final SpacecraftState state, final double[][] dY1dY0,
  220.                                     final double[][] dY1dP, final double[] p) {

  221.         // set up a converter
  222.         final RealMatrix dY1dC1 = MatrixUtils.createRealIdentityMatrix(STATE_DIMENSION);
  223.         final DecompositionSolver solver = new QRDecomposition(dY1dC1).getSolver();

  224.         // convert the provided state Jacobian
  225.         final RealMatrix dC1dY0 = solver.solve(new Array2DRowRealMatrix(dY1dY0, false));

  226.         // map the converted state Jacobian to one-dimensional array
  227.         int index = 0;
  228.         for (int i = 0; i < STATE_DIMENSION; ++i) {
  229.             for (int j = 0; j < STATE_DIMENSION; ++j) {
  230.                 p[index++] = dC1dY0.getEntry(i, j);
  231.             }
  232.         }

  233.         if (selected.getNbValuesToEstimate() != 0) {
  234.             // convert the provided state Jacobian
  235.             final RealMatrix dC1dP = solver.solve(new Array2DRowRealMatrix(dY1dP, false));

  236.             // map the converted parameters Jacobian to one-dimensional array
  237.             for (int i = 0; i < STATE_DIMENSION; ++i) {
  238.                 for (int j = 0; j < selected.getNbValuesToEstimate(); ++j) {
  239.                     p[index++] = dC1dP.getEntry(i, j);
  240.                 }
  241.             }
  242.         }

  243.     }

  244.     /** {@inheritDoc} */
  245.     public CombinedDerivatives combinedDerivatives(final SpacecraftState s) {

  246.         // initialize acceleration Jacobians to zero
  247.         final int paramDimEpoch = selected.getNbValuesToEstimate() + 1; // added epoch
  248.         final int dimEpoch      = 3;
  249.         final double[][] dAccdParam = new double[dimEpoch][paramDimEpoch];
  250.         final double[][] dAccdPos   = new double[dimEpoch][dimEpoch];
  251.         final double[][] dAccdVel   = new double[dimEpoch][dimEpoch];

  252.         final NumericalGradientConverter fullConverter    = new NumericalGradientConverter(s, 6, propagator.getAttitudeProvider());
  253.         final NumericalGradientConverter posOnlyConverter = new NumericalGradientConverter(s, 3, propagator.getAttitudeProvider());

  254.         // compute acceleration Jacobians, finishing with the largest force: Newtonian attraction
  255.         for (final ForceModel forceModel : propagator.getAllForceModels()) {
  256.             final NumericalGradientConverter converter = forceModel.dependsOnPositionOnly() ? posOnlyConverter : fullConverter;
  257.             final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
  258.             final Gradient[] parameters = converter.getParametersAtStateDate(dsState, forceModel);

  259.             final FieldVector3D<Gradient> acceleration = forceModel.acceleration(dsState, parameters);
  260.             final double[] derivativesX = acceleration.getX().getGradient();
  261.             final double[] derivativesY = acceleration.getY().getGradient();
  262.             final double[] derivativesZ = acceleration.getZ().getGradient();

  263.             // update Jacobians with respect to state
  264.             addToRow(derivativesX, 0, converter.getFreeStateParameters(), dAccdPos, dAccdVel);
  265.             addToRow(derivativesY, 1, converter.getFreeStateParameters(), dAccdPos, dAccdVel);
  266.             addToRow(derivativesZ, 2, converter.getFreeStateParameters(), dAccdPos, dAccdVel);

  267.             int index = converter.getFreeStateParameters();
  268.             for (ParameterDriver driver : forceModel.getParametersDrivers()) {
  269.                 if (driver.isSelected()) {
  270.                     for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
  271.                         final int parameterIndex = map.get(span.getData());
  272.                         dAccdParam[0][parameterIndex] += derivativesX[index];
  273.                         dAccdParam[1][parameterIndex] += derivativesY[index];
  274.                         dAccdParam[2][parameterIndex] += derivativesZ[index];
  275.                         ++index;
  276.                     }
  277.                 }
  278.             }

  279.             // Add the derivatives of the acceleration w.r.t. the Epoch
  280.             if (forceModel instanceof ThirdBodyAttractionEpoch) {
  281.                 final double[] parametersValues = new double[] {parameters[0].getValue()};
  282.                 final double[] derivatives = ((ThirdBodyAttractionEpoch) forceModel).getDerivativesToEpoch(s, parametersValues);
  283.                 dAccdParam[0][paramDimEpoch - 1] += derivatives[0];
  284.                 dAccdParam[1][paramDimEpoch - 1] += derivatives[1];
  285.                 dAccdParam[2][paramDimEpoch - 1] += derivatives[2];
  286.             }

  287.         }

  288.         // the variational equations of the complete state Jacobian matrix have the following form:

  289.         // [        |        ]   [                 |                  ]   [     |     ]
  290.         // [  Adot  |  Bdot  ]   [  dVel/dPos = 0  |  dVel/dVel = Id  ]   [  A  |  B  ]
  291.         // [        |        ]   [                 |                  ]   [     |     ]
  292.         // ---------+---------   ------------------+------------------- * ------+------
  293.         // [        |        ]   [                 |                  ]   [     |     ]
  294.         // [  Cdot  |  Ddot  ] = [    dAcc/dPos    |     dAcc/dVel    ]   [  C  |  D  ]
  295.         // [        |        ]   [                 |                  ]   [     |     ]

  296.         // The A, B, C and D sub-matrices and their derivatives (Adot ...) are 3x3 matrices

  297.         // The expanded multiplication above can be rewritten to take into account
  298.         // the fixed values found in the sub-matrices in the left factor. This leads to:

  299.         //     [ Adot ] = [ C ]
  300.         //     [ Bdot ] = [ D ]
  301.         //     [ Cdot ] = [ dAcc/dPos ] * [ A ] + [ dAcc/dVel ] * [ C ]
  302.         //     [ Ddot ] = [ dAcc/dPos ] * [ B ] + [ dAcc/dVel ] * [ D ]

  303.         // The following loops compute these expressions taking care of the mapping of the
  304.         // (A, B, C, D) matrices into the single dimension array p and of the mapping of the
  305.         // (Adot, Bdot, Cdot, Ddot) matrices into the single dimension array pDot.

  306.         // copy C and E into Adot and Bdot
  307.         final int stateDim = 6;
  308.         final double[] p = s.getAdditionalState(getName());
  309.         final double[] pDot = new double[p.length];
  310.         System.arraycopy(p, dimEpoch * stateDim, pDot, 0, dimEpoch * stateDim);

  311.         // compute Cdot and Ddot
  312.         for (int i = 0; i < dimEpoch; ++i) {
  313.             final double[] dAdPi = dAccdPos[i];
  314.             final double[] dAdVi = dAccdVel[i];
  315.             for (int j = 0; j < stateDim; ++j) {
  316.                 pDot[(dimEpoch + i) * stateDim + j] =
  317.                     dAdPi[0] * p[j]                + dAdPi[1] * p[j +     stateDim] + dAdPi[2] * p[j + 2 * stateDim] +
  318.                     dAdVi[0] * p[j + 3 * stateDim] + dAdVi[1] * p[j + 4 * stateDim] + dAdVi[2] * p[j + 5 * stateDim];
  319.             }
  320.         }

  321.         for (int k = 0; k < paramDimEpoch; ++k) {
  322.             // the variational equations of the parameters Jacobian matrix are computed
  323.             // one column at a time, they have the following form:
  324.             // [      ]   [                 |                  ]   [   ]   [                  ]
  325.             // [ Edot ]   [  dVel/dPos = 0  |  dVel/dVel = Id  ]   [ E ]   [  dVel/dParam = 0 ]
  326.             // [      ]   [                 |                  ]   [   ]   [                  ]
  327.             // --------   ------------------+------------------- * ----- + --------------------
  328.             // [      ]   [                 |                  ]   [   ]   [                  ]
  329.             // [ Fdot ] = [    dAcc/dPos    |     dAcc/dVel    ]   [ F ]   [    dAcc/dParam   ]
  330.             // [      ]   [                 |                  ]   [   ]   [                  ]

  331.             // The E and F sub-columns and their derivatives (Edot, Fdot) are 3 elements columns.

  332.             // The expanded multiplication and addition above can be rewritten to take into
  333.             // account the fixed values found in the sub-matrices in the left factor. This leads to:

  334.             //     [ Edot ] = [ F ]
  335.             //     [ Fdot ] = [ dAcc/dPos ] * [ E ] + [ dAcc/dVel ] * [ F ] + [ dAcc/dParam ]

  336.             // The following loops compute these expressions taking care of the mapping of the
  337.             // (E, F) columns into the single dimension array p and of the mapping of the
  338.             // (Edot, Fdot) columns into the single dimension array pDot.

  339.             // copy F into Edot
  340.             final int columnTop = stateDim * stateDim + k;
  341.             pDot[columnTop]                     = p[columnTop + 3 * paramDimEpoch];
  342.             pDot[columnTop +     paramDimEpoch] = p[columnTop + 4 * paramDimEpoch];
  343.             pDot[columnTop + 2 * paramDimEpoch] = p[columnTop + 5 * paramDimEpoch];

  344.             // compute Fdot
  345.             for (int i = 0; i < dimEpoch; ++i) {
  346.                 final double[] dAdP = dAccdPos[i];
  347.                 final double[] dAdV = dAccdVel[i];
  348.                 pDot[columnTop + (dimEpoch + i) * paramDimEpoch] =
  349.                     dAccdParam[i][k] +
  350.                     dAdP[0] * p[columnTop]                     + dAdP[1] * p[columnTop +     paramDimEpoch] + dAdP[2] * p[columnTop + 2 * paramDimEpoch] +
  351.                     dAdV[0] * p[columnTop + 3 * paramDimEpoch] + dAdV[1] * p[columnTop + 4 * paramDimEpoch] + dAdV[2] * p[columnTop + 5 * paramDimEpoch];
  352.             }

  353.         }

  354.         return new CombinedDerivatives(pDot, null);

  355.     }

  356.     /** Fill Jacobians rows.
  357.      * @param derivatives derivatives of a component of acceleration (along either x, y or z)
  358.      * @param index component index (0 for x, 1 for y, 2 for z)
  359.      * @param freeStateParameters number of free parameters, either 3 (position),
  360.      * 6 (position-velocity) or 7 (position-velocity-mass)
  361.      * @param dAccdPos Jacobian of acceleration with respect to spacecraft position
  362.      * @param dAccdVel Jacobian of acceleration with respect to spacecraft velocity
  363.      */
  364.     private void addToRow(final double[] derivatives, final int index, final int freeStateParameters,
  365.                           final double[][] dAccdPos, final double[][] dAccdVel) {

  366.         for (int i = 0; i < 3; ++i) {
  367.             dAccdPos[index][i] += derivatives[i];
  368.         }
  369.         if (freeStateParameters > 3) {
  370.             for (int i = 0; i < 3; ++i) {
  371.                 dAccdVel[index][i] += derivatives[i + 3];
  372.             }
  373.         }

  374.     }

  375. }