DSSTPartialDerivativesEquations.java

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

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

  20. import org.hipparchus.analysis.differentiation.Gradient;
  21. import org.orekit.errors.OrekitException;
  22. import org.orekit.errors.OrekitMessages;
  23. import org.orekit.propagation.FieldSpacecraftState;
  24. import org.orekit.propagation.PropagationType;
  25. import org.orekit.propagation.SpacecraftState;
  26. import org.orekit.propagation.integration.AdditionalDerivativesProvider;
  27. import org.orekit.propagation.semianalytical.dsst.forces.DSSTForceModel;
  28. import org.orekit.propagation.semianalytical.dsst.utilities.FieldAuxiliaryElements;
  29. import org.orekit.time.AbsoluteDate;
  30. import org.orekit.utils.ParameterDriver;
  31. import org.orekit.utils.ParameterDriversList;

  32. /** {@link AdditionalDerivativesProvider derivatives provider} computing the partial derivatives
  33.  * of the state (orbit) with respect to initial state and force models parameters.
  34.  * <p>
  35.  * This set of equations are automatically added to a {@link DSSTPropagator DSST propagator}
  36.  * in order to compute partial derivatives of the orbit along with the orbit itself. This is
  37.  * useful for example in orbit determination applications.
  38.  * </p>
  39.  * <p>
  40.  * The partial derivatives with respect to initial state are dimension 6 (orbit only).
  41.  * </p>
  42.  * <p>
  43.  * The partial derivatives with respect to force models parameters has a dimension
  44.  * equal to the number of selected parameters. Parameters selection is implemented at
  45.  * {@link DSSTForceModel DSST force models} level. Users must retrieve a {@link ParameterDriver
  46.  * parameter driver} by looping on all drivers using {@link DSSTForceModel#getParametersDrivers()}
  47.  * and then select it by calling {@link ParameterDriver#setSelected(boolean) setSelected(true)}.
  48.  * </p>
  49.  * @author Bryan Cazabonne
  50.  * @since 10.0
  51.  * @deprecated as of 11.1, replaced by {@link
  52.  * org.orekit.propagation.Propagator#setupMatricesComputation(String,
  53.  * org.hipparchus.linear.RealMatrix, org.orekit.utils.DoubleArrayDictionary)}
  54.  */
  55. @Deprecated
  56. public class DSSTPartialDerivativesEquations
  57.     implements AdditionalDerivativesProvider,
  58.                org.orekit.propagation.integration.AdditionalEquations {

  59.     /** Retrograde factor I.
  60.      *  <p>
  61.      *  DSST model needs equinoctial orbit as internal representation.
  62.      *  Classical equinoctial elements have discontinuities when inclination
  63.      *  is close to zero. In this representation, I = +1. <br>
  64.      *  To avoid this discontinuity, another representation exists and equinoctial
  65.      *  elements can be expressed in a different way, called "retrograde" orbit.
  66.      *  This implies I = -1. <br>
  67.      *  As Orekit doesn't implement the retrograde orbit, I is always set to +1.
  68.      *  But for the sake of consistency with the theory, the retrograde factor
  69.      *  has been kept in the formulas.
  70.      *  </p>
  71.      */
  72.     private static final int I = 1;

  73.     /** Propagator computing state evolution. */
  74.     private final DSSTPropagator propagator;

  75.     /** Selected parameters for Jacobian computation. */
  76.     private ParameterDriversList selected;

  77.     /** Parameters map. */
  78.     private Map<ParameterDriver, Integer> map;

  79.     /** Name. */
  80.     private final String name;

  81.     /** Flag for Jacobian matrices initialization. */
  82.     private boolean initialized;

  83.     /** Type of the orbit used for the propagation.*/
  84.     private PropagationType propagationType;

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

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

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

  117.     /** Freeze the selected parameters from the force models.
  118.      */
  119.     private void freezeParametersSelection() {
  120.         if (selected == null) {

  121.             // first pass: gather all parameters, binding similar names together
  122.             selected = new ParameterDriversList();
  123.             for (final DSSTForceModel provider : propagator.getAllForceModels()) {
  124.                 for (final ParameterDriver driver : provider.getParametersDrivers()) {
  125.                     selected.add(driver);
  126.                 }
  127.             }

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

  131.             // third pass: sort parameters lexicographically
  132.             selected.sort();

  133.             // fourth pass: set up a map between parameters drivers and matrices columns
  134.             map = new IdentityHashMap<ParameterDriver, Integer>();
  135.             int parameterIndex = 0;
  136.             for (final ParameterDriver selectedDriver : selected.getDrivers()) {
  137.                 for (final DSSTForceModel provider : propagator.getAllForceModels()) {
  138.                     for (final ParameterDriver driver : provider.getParametersDrivers()) {
  139.                         if (driver.getName().equals(selectedDriver.getName())) {
  140.                             map.put(driver, parameterIndex);
  141.                         }
  142.                     }
  143.                 }
  144.                 ++parameterIndex;
  145.             }

  146.         }
  147.     }

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

  172.     /** Set the initial value of the Jacobian with respect to state and parameter.
  173.      * <p>
  174.      * The returned state must be added to the propagator (it is not done
  175.      * automatically, as the user may need to add more states to it).
  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, and the {@code dY1dP} matrix dimension <em>must</em>
  181.      * be consistent with the selection.
  182.      * </p>
  183.      * @param s1 current state
  184.      * @param dY1dY0 Jacobian of current state at time t₁ with respect
  185.      * to state at some previous time t₀ (must be 6x6)
  186.      * @param dY1dP Jacobian of current state at time t₁ with respect
  187.      * to parameters (may be null if no parameters are selected)
  188.      * @return state with initial Jacobians added
  189.      */
  190.     public SpacecraftState setInitialJacobians(final SpacecraftState s1,
  191.                                                final double[][] dY1dY0, final double[][] dY1dP) {

  192.         freezeParametersSelection();

  193.         // Check dimensions
  194.         final int stateDim = dY1dY0.length;
  195.         if (stateDim != 6 || stateDim != dY1dY0[0].length) {
  196.             throw new OrekitException(OrekitMessages.STATE_JACOBIAN_NOT_6X6,
  197.                                       stateDim, dY1dY0[0].length);
  198.         }
  199.         if (dY1dP != null && stateDim != dY1dP.length) {
  200.             throw new OrekitException(OrekitMessages.STATE_AND_PARAMETERS_JACOBIANS_ROWS_MISMATCH,
  201.                                       stateDim, dY1dP.length);
  202.         }
  203.         if (dY1dP == null && selected.getNbParams() != 0 ||
  204.             dY1dP != null && selected.getNbParams() != dY1dP[0].length) {
  205.             throw new OrekitException(new OrekitException(OrekitMessages.INITIAL_MATRIX_AND_PARAMETERS_NUMBER_MISMATCH,
  206.                                                           dY1dP == null ? 0 : dY1dP[0].length, selected.getNbParams()));
  207.         }

  208.         // store the matrices as a single dimension array
  209.         initialized = true;
  210.         final DSSTJacobiansMapper mapper = getMapper();
  211.         final double[] p = new double[mapper.getAdditionalStateDimension()];
  212.         mapper.setInitialJacobians(s1, dY1dY0, dY1dP, p);

  213.         // set value in propagator
  214.         return s1.addAdditionalState(name, p);

  215.     }

  216.     /** Get a mapper between two-dimensional Jacobians and one-dimensional additional state.
  217.      * @return a mapper between two-dimensional Jacobians and one-dimensional additional state,
  218.      * with the same name as the instance
  219.      * @see #setInitialJacobians(SpacecraftState)
  220.      * @see #setInitialJacobians(SpacecraftState, double[][], double[][])
  221.      */
  222.     public DSSTJacobiansMapper getMapper() {
  223.         if (!initialized) {
  224.             throw new OrekitException(OrekitMessages.STATE_JACOBIAN_NOT_INITIALIZED);
  225.         }
  226.         return new DSSTJacobiansMapper(name, selected, propagator, map, propagationType);
  227.     }

  228.     /** {@inheritDoc} */
  229.     public void init(final SpacecraftState initialState, final AbsoluteDate target) {
  230.         // FIXME: remove in 12.0 when AdditionalEquations is removed
  231.         AdditionalDerivativesProvider.super.init(initialState, target);
  232.     }

  233.     /** {@inheritDoc} */
  234.     public double[] computeDerivatives(final SpacecraftState s, final double[] pDot) {
  235.         // FIXME: remove in 12.0 when AdditionalEquations is removed
  236.         System.arraycopy(derivatives(s), 0, pDot, 0, pDot.length);
  237.         return null;
  238.     }

  239.     /** {@inheritDoc} */
  240.     public double[] derivatives(final SpacecraftState s) {

  241.         // initialize Jacobians to zero
  242.         final int paramDim = selected.getNbParams();
  243.         final int dim = 6;
  244.         final double[][] dMeanElementRatedParam   = new double[dim][paramDim];
  245.         final double[][] dMeanElementRatedElement = new double[dim][dim];
  246.         final DSSTGradientConverter converter = new DSSTGradientConverter(s, propagator.getAttitudeProvider());

  247.         // Compute Jacobian
  248.         for (final DSSTForceModel forceModel : propagator.getAllForceModels()) {

  249.             final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
  250.             final Gradient[] parameters = converter.getParameters(dsState, forceModel);
  251.             final FieldAuxiliaryElements<Gradient> auxiliaryElements = new FieldAuxiliaryElements<>(dsState.getOrbit(), I);

  252.             // "field" initialization of the force model if it was not done before
  253.             forceModel.initializeShortPeriodTerms(auxiliaryElements, propagationType, parameters);
  254.             final Gradient[] meanElementRate = forceModel.getMeanElementRate(dsState, auxiliaryElements, parameters);
  255.             final double[] derivativesA  = meanElementRate[0].getGradient();
  256.             final double[] derivativesEx = meanElementRate[1].getGradient();
  257.             final double[] derivativesEy = meanElementRate[2].getGradient();
  258.             final double[] derivativesHx = meanElementRate[3].getGradient();
  259.             final double[] derivativesHy = meanElementRate[4].getGradient();
  260.             final double[] derivativesL  = meanElementRate[5].getGradient();

  261.             // update Jacobian with respect to state
  262.             addToRow(derivativesA,  0, dMeanElementRatedElement);
  263.             addToRow(derivativesEx, 1, dMeanElementRatedElement);
  264.             addToRow(derivativesEy, 2, dMeanElementRatedElement);
  265.             addToRow(derivativesHx, 3, dMeanElementRatedElement);
  266.             addToRow(derivativesHy, 4, dMeanElementRatedElement);
  267.             addToRow(derivativesL,  5, dMeanElementRatedElement);

  268.             int index = converter.getFreeStateParameters();
  269.             for (ParameterDriver driver : forceModel.getParametersDrivers()) {
  270.                 if (driver.isSelected()) {
  271.                     final int parameterIndex = map.get(driver);
  272.                     dMeanElementRatedParam[0][parameterIndex] += derivativesA[index];
  273.                     dMeanElementRatedParam[1][parameterIndex] += derivativesEx[index];
  274.                     dMeanElementRatedParam[2][parameterIndex] += derivativesEy[index];
  275.                     dMeanElementRatedParam[3][parameterIndex] += derivativesHx[index];
  276.                     dMeanElementRatedParam[4][parameterIndex] += derivativesHy[index];
  277.                     dMeanElementRatedParam[5][parameterIndex] += derivativesL[index];
  278.                     ++index;
  279.                 }
  280.             }

  281.         }

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

  283.         //                     [ Adot ] = [ dMeanElementRatedElement ] * [ A ]

  284.         // The A matrix and its derivative (Adot) are 6 * 6 matrices

  285.         // The following loops compute these expression taking care of the mapping of the
  286.         // A matrix into the single dimension array p and of the mapping of the
  287.         // Adot matrix into the single dimension array pDot.

  288.         final double[] p = s.getAdditionalState(getName());
  289.         final double[] pDot = new double[p.length];

  290.         for (int i = 0; i < dim; i++) {
  291.             final double[] dMeanElementRatedElementi = dMeanElementRatedElement[i];
  292.             for (int j = 0; j < dim; j++) {
  293.                 pDot[j + dim * i] =
  294.                     dMeanElementRatedElementi[0] * p[j]           + dMeanElementRatedElementi[1] * p[j +     dim] + dMeanElementRatedElementi[2] * p[j + 2 * dim] +
  295.                     dMeanElementRatedElementi[3] * p[j + 3 * dim] + dMeanElementRatedElementi[4] * p[j + 4 * dim] + dMeanElementRatedElementi[5] * p[j + 5 * dim];
  296.             }
  297.         }

  298.         final int columnTop = dim * dim;
  299.         for (int k = 0; k < paramDim; k++) {
  300.             // the variational equations of the parameters Jacobian matrix are computed
  301.             // one column at a time, they have the following form:

  302.             //             [ Bdot ] = [ dMeanElementRatedElement ] * [ B ] + [ dMeanElementRatedParam ]

  303.             // The B sub-columns and its derivative (Bdot) are 6 elements columns.

  304.             // The following loops compute this expression taking care of the mapping of the
  305.             // B columns into the single dimension array p and of the mapping of the
  306.             // Bdot columns into the single dimension array pDot.

  307.             for (int i = 0; i < dim; ++i) {
  308.                 final double[] dMeanElementRatedElementi = dMeanElementRatedElement[i];
  309.                 pDot[columnTop + (i + dim * k)] =
  310.                     dMeanElementRatedParam[i][k] +
  311.                     dMeanElementRatedElementi[0] * p[columnTop + k]                + dMeanElementRatedElementi[1] * p[columnTop + k +     paramDim] + dMeanElementRatedElementi[2] * p[columnTop + k + 2 * paramDim] +
  312.                     dMeanElementRatedElementi[3] * p[columnTop + k + 3 * paramDim] + dMeanElementRatedElementi[4] * p[columnTop + k + 4 * paramDim] + dMeanElementRatedElementi[5] * p[columnTop + k + 5 * paramDim];
  313.             }
  314.         }

  315.         return pDot;

  316.     }

  317.     /** Fill Jacobians rows.
  318.      * @param derivatives derivatives of a component
  319.      * @param index component index (0 for a, 1 for ex, 2 for ey, 3 for hx, 4 for hy, 5 for l)
  320.      * @param dMeanElementRatedElement Jacobian of mean elements rate with respect to mean elements
  321.      */
  322.     private void addToRow(final double[] derivatives, final int index,
  323.                           final double[][] dMeanElementRatedElement) {

  324.         for (int i = 0; i < 6; i++) {
  325.             dMeanElementRatedElement[index][i] += derivatives[i];
  326.         }

  327.     }

  328. }