PartialDerivativesEquations.java

  1. /* Copyright 2010-2011 Centre National d'Études Spatiales
  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.IdentityHashMap;
  20. import java.util.Map;

  22. import org.hipparchus.analysis.differentiation.Gradient;
  23. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  24. import org.orekit.errors.OrekitException;
  25. import org.orekit.errors.OrekitMessages;
  26. import org.orekit.forces.ForceModel;
  27. import org.orekit.propagation.FieldSpacecraftState;
  28. import org.orekit.propagation.SpacecraftState;
  29. import org.orekit.propagation.integration.AdditionalDerivativesProvider;
  30. import org.orekit.propagation.integration.CombinedDerivatives;
  31. import org.orekit.time.AbsoluteDate;
  32. import org.orekit.utils.ParameterDriver;
  33. import org.orekit.utils.ParameterDriversList;

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

  74.     /** Propagator computing state evolution. */
  75.     private final NumericalPropagator propagator;

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

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

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

  86.     /** Flag for Jacobian matrices initialization. */
  87.     private boolean initialized;

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

  108.     /** {@inheritDoc} */
  109.     public String getName() {
  110.         return name;
  111.     }

  113.     /** {@inheritDoc} */
  114.     @Override
  115.     public int getDimension() {
  116.         freezeParametersSelection();
  117.         return 6 * (6 + selected.getNbParams());
  118.     }

  120.     /** Freeze the selected parameters from the force models.
  121.      */
  122.     private void freezeParametersSelection() {
  123.         if (selected == null) {

  125.             // first pass: gather all parameters, binding similar names together
  126.             selected = new ParameterDriversList();
  127.             for (final ForceModel provider : propagator.getAllForceModels()) {
  128.                 for (final ParameterDriver driver : provider.getParametersDrivers()) {
  129.                     selected.add(driver);
  130.                 }
  131.             }

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

  137.             // third pass: sort parameters lexicographically
  138.             selected.sort();

  140.             // fourth pass: set up a map between parameters drivers and matrices columns
  141.             map = new IdentityHashMap<ParameterDriver, Integer>();
  142.             int parameterIndex = 0;
  143.             for (final ParameterDriver selectedDriver : selected.getDrivers()) {
  144.                 for (final ForceModel provider : propagator.getAllForceModels()) {
  145.                     for (final ParameterDriver driver : provider.getParametersDrivers()) {
  146.                         if (driver.getName().equals(selectedDriver.getName())) {
  147.                             map.put(driver, parameterIndex);
  148.                         }
  149.                     }
  150.                 }
  151.                 ++parameterIndex;
  152.             }

  154.         }
  155.     }

  157.     /** Get the selected parameters, in Jacobian matrix column order.
  158.      * <p>
  159.      * The force models parameters for which partial derivatives are desired,
  160.      * <em>must</em> have been {@link ParameterDriver#setSelected(boolean) selected}
  161.      * before this method is called, so the proper list is returned.
  162.      * </p>
  163.      * @return selected parameters, in Jacobian matrix column order which
  164.      * is lexicographic order
  165.      */
  166.     public ParameterDriversList getSelectedParameters() {
  167.         freezeParametersSelection();
  168.         return selected;
  169.     }

  171.     /** Set the initial value of the Jacobian with respect to state and parameter.
  172.      * <p>
  173.      * This method is equivalent to call {@link #setInitialJacobians(SpacecraftState,
  174.      * double[][], double[][])} with dYdY0 set to the identity matrix and dYdP set
  175.      * to a zero matrix.
  176.      * </p>
  177.      * <p>
  178.      * The force models parameters for which partial derivatives are desired,
  179.      * <em>must</em> have been {@link ParameterDriver#setSelected(boolean) selected}
  180.      * before this method is called, so proper matrices dimensions are used.
  181.      * </p>
  182.      * @param s0 initial state
  183.      * @return state with initial Jacobians added
  184.      * @see #getSelectedParameters()
  185.      * @since 9.0
  186.      */
  187.     public SpacecraftState setInitialJacobians(final SpacecraftState s0) {
  188.         freezeParametersSelection();
  189.         final int stateDimension = 6;
  190.         final double[][] dYdY0 = new double[stateDimension][stateDimension];
  191.         final double[][] dYdP  = new double[stateDimension][selected.getNbParams()];
  192.         for (int i = 0; i < stateDimension; ++i) {
  193.             dYdY0[i][i] = 1.0;
  194.         }
  195.         return setInitialJacobians(s0, dYdY0, dYdP);
  196.     }

  198.     /** Set the initial value of the Jacobian with respect to state and parameter.
  199.      * <p>
  200.      * The returned state must be added to the propagator (it is not done
  201.      * automatically, as the user may need to add more states to it).
  202.      * </p>
  203.      * <p>
  204.      * The force models parameters for which partial derivatives are desired,
  205.      * <em>must</em> have been {@link ParameterDriver#setSelected(boolean) selected}
  206.      * before this method is called, and the {@code dY1dP} matrix dimension <em>must</em>
  207.      * be consistent with the selection.
  208.      * </p>
  209.      * @param s1 current state
  210.      * @param dY1dY0 Jacobian of current state at time t₁ with respect
  211.      * to state at some previous time t₀ (must be 6x6)
  212.      * @param dY1dP Jacobian of current state at time t₁ with respect
  213.      * to parameters (may be null if no parameters are selected)
  214.      * @return state with initial Jacobians added
  215.      * @see #getSelectedParameters()
  216.      */
  217.     public SpacecraftState setInitialJacobians(final SpacecraftState s1,
  218.                                                final double[][] dY1dY0, final double[][] dY1dP) {

  220.         freezeParametersSelection();

  222.         // Check dimensions
  223.         final int stateDim = dY1dY0.length;
  224.         if (stateDim != 6 || stateDim != dY1dY0[0].length) {
  225.             throw new OrekitException(OrekitMessages.STATE_JACOBIAN_NOT_6X6,
  226.                                       stateDim, dY1dY0[0].length);
  227.         }
  228.         if (dY1dP != null && stateDim != dY1dP.length) {
  229.             throw new OrekitException(OrekitMessages.STATE_AND_PARAMETERS_JACOBIANS_ROWS_MISMATCH,
  230.                                       stateDim, dY1dP.length);
  231.         }
  232.         if (dY1dP == null && selected.getNbParams() != 0 ||
  233.             dY1dP != null && selected.getNbParams() != dY1dP[0].length) {
  234.             throw new OrekitException(new OrekitException(OrekitMessages.INITIAL_MATRIX_AND_PARAMETERS_NUMBER_MISMATCH,
  235.                                                           dY1dP == null ? 0 : dY1dP[0].length, selected.getNbParams()));
  236.         }

  238.         // store the matrices as a single dimension array
  239.         initialized = true;
  240.         final JacobiansMapper mapper = getMapper();
  241.         final double[] p = new double[mapper.getAdditionalStateDimension()];
  242.         mapper.setInitialJacobians(s1, dY1dY0, dY1dP, p);

  244.         // set value in propagator
  245.         return s1.addAdditionalState(name, p);

  247.     }

  249.     /** Get a mapper between two-dimensional Jacobians and one-dimensional additional state.
  250.      * @return a mapper between two-dimensional Jacobians and one-dimensional additional state,
  251.      * with the same name as the instance
  252.      * @see #setInitialJacobians(SpacecraftState)
  253.      * @see #setInitialJacobians(SpacecraftState, double[][], double[][])
  254.      */
  255.     public JacobiansMapper getMapper() {
  256.         if (!initialized) {
  257.             throw new OrekitException(OrekitMessages.STATE_JACOBIAN_NOT_INITIALIZED);
  258.         }
  259.         return new JacobiansMapper(name, selected,
  260.                                    propagator.getOrbitType(),
  261.                                    propagator.getPositionAngleType());
  262.     }

  264.     /** {@inheritDoc} */
  265.     public void init(final SpacecraftState initialState, final AbsoluteDate target) {
  266.         // FIXME: remove in 12.0 when AdditionalEquations is removed
  267.         AdditionalDerivativesProvider.super.init(initialState, target);
  268.     }

  270.     /** {@inheritDoc} */
  271.     public double[] computeDerivatives(final SpacecraftState s, final double[] pDot) {
  272.         // FIXME: remove in 12.0 when AdditionalEquations is removed
  273.         System.arraycopy(derivatives(s), 0, pDot, 0, pDot.length);
  274.         return null;
  275.     }

  277.     /** {@inheritDoc} */
  278.     @Override
  279.     @Deprecated
  280.     public double[] derivatives(final SpacecraftState state) {
  281.         return combinedDerivatives(state).getAdditionalDerivatives();
  282.     }

  284.     /** {@inheritDoc} */
  285.     public CombinedDerivatives combinedDerivatives(final SpacecraftState s) {

  287.         // initialize acceleration Jacobians to zero
  288.         final int paramDim = selected.getNbParams();
  289.         final int dim = 3;
  290.         final double[][] dAccdParam = new double[dim][paramDim];
  291.         final double[][] dAccdPos   = new double[dim][dim];
  292.         final double[][] dAccdVel   = new double[dim][dim];

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

  297.         // compute acceleration Jacobians, finishing with the largest force: Newtonian attraction
  298.         for (final ForceModel forceModel : propagator.getAllForceModels()) {

  300.             final NumericalGradientConverter converter = forceModel.dependsOnPositionOnly() ? posOnlyConverter : fullConverter;
  301.             final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
  302.             final Gradient[] parameters = converter.getParameters(dsState, forceModel);

  304.             final FieldVector3D<Gradient> acceleration = forceModel.acceleration(dsState, parameters);
  305.             final double[] derivativesX = acceleration.getX().getGradient();
  306.             final double[] derivativesY = acceleration.getY().getGradient();
  307.             final double[] derivativesZ = acceleration.getZ().getGradient();

  309.             // update Jacobians with respect to state
  310.             addToRow(derivativesX, 0, converter.getFreeStateParameters(), dAccdPos, dAccdVel);
  311.             addToRow(derivativesY, 1, converter.getFreeStateParameters(), dAccdPos, dAccdVel);
  312.             addToRow(derivativesZ, 2, converter.getFreeStateParameters(), dAccdPos, dAccdVel);

  314.             int index = converter.getFreeStateParameters();
  315.             for (ParameterDriver driver : forceModel.getParametersDrivers()) {
  316.                 if (driver.isSelected()) {
  317.                     final int parameterIndex = map.get(driver);
  318.                     dAccdParam[0][parameterIndex] += derivativesX[index];
  319.                     dAccdParam[1][parameterIndex] += derivativesY[index];
  320.                     dAccdParam[2][parameterIndex] += derivativesZ[index];
  321.                     ++index;
  322.                 }
  323.             }

  325.         }

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

  329.         // [        |        ]   [                 |                  ]   [     |     ]
  330.         // [  Adot  |  Bdot  ]   [  dVel/dPos = 0  |  dVel/dVel = Id  ]   [  A  |  B  ]
  331.         // [        |        ]   [                 |                  ]   [     |     ]
  332.         // ---------+---------   ------------------+------------------- * ------+------
  333.         // [        |        ]   [                 |                  ]   [     |     ]
  334.         // [  Cdot  |  Ddot  ] = [    dAcc/dPos    |     dAcc/dVel    ]   [  C  |  D  ]
  335.         // [        |        ]   [                 |                  ]   [     |     ]

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

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

  342.         //     [ Adot ] = [ C ]
  343.         //     [ Bdot ] = [ D ]
  344.         //     [ Cdot ] = [ dAcc/dPos ] * [ A ] + [ dAcc/dVel ] * [ C ]
  345.         //     [ Ddot ] = [ dAcc/dPos ] * [ B ] + [ dAcc/dVel ] * [ D ]

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

  351.         // copy C and E into Adot and Bdot
  352.         final int stateDim = 6;
  353.         final double[] p = s.getAdditionalState(getName());
  354.         final double[] pDot = new double[p.length];
  355.         System.arraycopy(p, dim * stateDim, pDot, 0, dim * stateDim);

  357.         // compute Cdot and Ddot
  358.         for (int i = 0; i < dim; ++i) {
  359.             final double[] dAdPi = dAccdPos[i];
  360.             final double[] dAdVi = dAccdVel[i];
  361.             for (int j = 0; j < stateDim; ++j) {
  362.                 pDot[(dim + i) * stateDim + j] =
  363.                     dAdPi[0] * p[j]                + dAdPi[1] * p[j +     stateDim] + dAdPi[2] * p[j + 2 * stateDim] +
  364.                     dAdVi[0] * p[j + 3 * stateDim] + dAdVi[1] * p[j + 4 * stateDim] + dAdVi[2] * p[j + 5 * stateDim];
  365.             }
  366.         }

  368.         for (int k = 0; k < paramDim; ++k) {
  369.             // the variational equations of the parameters Jacobian matrix are computed
  370.             // one column at a time, they have the following form:
  371.             // [      ]   [                 |                  ]   [   ]   [                  ]
  372.             // [ Edot ]   [  dVel/dPos = 0  |  dVel/dVel = Id  ]   [ E ]   [  dVel/dParam = 0 ]
  373.             // [      ]   [                 |                  ]   [   ]   [                  ]
  374.             // --------   ------------------+------------------- * ----- + --------------------
  375.             // [      ]   [                 |                  ]   [   ]   [                  ]
  376.             // [ Fdot ] = [    dAcc/dPos    |     dAcc/dVel    ]   [ F ]   [    dAcc/dParam   ]
  377.             // [      ]   [                 |                  ]   [   ]   [                  ]

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

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

  384.             //     [ Edot ] = [ F ]
  385.             //     [ Fdot ] = [ dAcc/dPos ] * [ E ] + [ dAcc/dVel ] * [ F ] + [ dAcc/dParam ]

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

  391.             // copy F into Edot
  392.             final int columnTop = stateDim * stateDim + k;
  393.             pDot[columnTop]                = p[columnTop + 3 * paramDim];
  394.             pDot[columnTop +     paramDim] = p[columnTop + 4 * paramDim];
  395.             pDot[columnTop + 2 * paramDim] = p[columnTop + 5 * paramDim];

  397.             // compute Fdot
  398.             for (int i = 0; i < dim; ++i) {
  399.                 final double[] dAdPi = dAccdPos[i];
  400.                 final double[] dAdVi = dAccdVel[i];
  401.                 pDot[columnTop + (dim + i) * paramDim] =
  402.                     dAccdParam[i][k] +
  403.                     dAdPi[0] * p[columnTop]                + dAdPi[1] * p[columnTop +     paramDim] + dAdPi[2] * p[columnTop + 2 * paramDim] +
  404.                     dAdVi[0] * p[columnTop + 3 * paramDim] + dAdVi[1] * p[columnTop + 4 * paramDim] + dAdVi[2] * p[columnTop + 5 * paramDim];
  405.             }

  407.         }

  409.         return new CombinedDerivatives(pDot, null);

  411.     }

  413.     /** Get the flag for the initialization of the state jacobian.
  414.      * @return true if the state jacobian is initialized
  415.      * @since 10.2
  416.      */
  417.     public boolean isInitialize() {
  418.         return initialized;
  419.     }

  421.     /** Fill Jacobians rows.
  422.      * @param derivatives derivatives of a component of acceleration (along either x, y or z)
  423.      * @param index component index (0 for x, 1 for y, 2 for z)
  424.      * @param freeStateParameters number of free parameters, either 3 (position),
  425.      * 6 (position-velocity) or 7 (position-velocity-mass)
  426.      * @param dAccdPos Jacobian of acceleration with respect to spacecraft position
  427.      * @param dAccdVel Jacobian of acceleration with respect to spacecraft velocity
  428.      */
  429.     private void addToRow(final double[] derivatives, final int index, final int freeStateParameters,
  430.                           final double[][] dAccdPos, final double[][] dAccdVel) {

  432.         for (int i = 0; i < 3; ++i) {
  433.             dAccdPos[index][i] += derivatives[i];
  434.         }
  435.         if (freeStateParameters > 3) {
  436.             for (int i = 0; i < 3; ++i) {
  437.                 dAccdVel[index][i] += derivatives[i + 3];
  438.             }
  439.         }

  441.     }

  443. }
Created with JaCoCo 0.8.8.202204050719