DSSTJacobiansMapper.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.ArrayList;
  19. import java.util.Arrays;
  20. import java.util.List;
  21. import java.util.Map;

  22. import org.hipparchus.analysis.differentiation.Gradient;
  23. import org.orekit.errors.OrekitInternalError;
  24. import org.orekit.propagation.FieldSpacecraftState;
  25. import org.orekit.propagation.PropagationType;
  26. import org.orekit.propagation.SpacecraftState;
  27. import org.orekit.propagation.integration.AbstractJacobiansMapper;
  28. import org.orekit.propagation.semianalytical.dsst.forces.DSSTForceModel;
  29. import org.orekit.propagation.semianalytical.dsst.forces.FieldShortPeriodTerms;
  30. import org.orekit.propagation.semianalytical.dsst.utilities.FieldAuxiliaryElements;
  31. import org.orekit.utils.ParameterDriver;
  32. import org.orekit.utils.ParameterDriversList;

  33. /** Mapper between two-dimensional Jacobian matrices and one-dimensional {@link
  34.  * SpacecraftState#getAdditionalState(String) additional state arrays}.
  35.  * <p>
  36.  * This class does not hold the states by itself. Instances of this class are guaranteed
  37.  * to be immutable.
  38.  * </p>
  39.  * @author Luc Maisonobe
  40.  * @author Bryan Cazabonne
  41.  * @see org.orekit.propagation.semianalytical.dsst.DSSTPartialDerivativesEquations
  42.  * @see org.orekit.propagation.semianalytical.dsst.DSSTPropagator
  43.  * @see SpacecraftState#getAdditionalState(String)
  44.  * @see org.orekit.propagation.AbstractPropagator
  45.  */
  46. public class DSSTJacobiansMapper extends AbstractJacobiansMapper {

  47.     /** State dimension, fixed to 6.
  48.      * @since 9.0
  49.      */
  50.     public static final int STATE_DIMENSION = 6;

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

  65.     /** Name. */
  66.     private String name;

  67.     /** Selected parameters for Jacobian computation. */
  68.     private final ParameterDriversList parameters;

  69.     /** Parameters map. */
  70.     private Map<ParameterDriver, Integer> map;

  71.     /** Propagator computing state evolution. */
  72.     private final DSSTPropagator propagator;

  73.     /** Placeholder for the derivatives of the short period terms.*/
  74.     private double[] shortPeriodDerivatives;

  75.     /** Type of the orbit used for the propagation.*/
  76.     private PropagationType propagationType;

  77.     /** Simple constructor.
  78.      * @param name name of the Jacobians
  79.      * @param parameters selected parameters for Jacobian computation
  80.      * @param propagator the propagator that will handle the orbit propagation
  81.      * @param map parameters map
  82.      * @param propagationType type of the orbit used for the propagation (mean or osculating)
  83.      */
  84.     DSSTJacobiansMapper(final String name,
  85.                         final ParameterDriversList parameters,
  86.                         final DSSTPropagator propagator,
  87.                         final Map<ParameterDriver, Integer> map,
  88.                         final PropagationType propagationType) {

  89.         super(name, parameters);

  90.         shortPeriodDerivatives = null;

  91.         this.parameters      = parameters;
  92.         this.name            = name;
  93.         this.propagator      = propagator;
  94.         this.map             = map;
  95.         this.propagationType = propagationType;

  96.     }

  97.     /** {@inheritDoc} */
  98.     public void setInitialJacobians(final SpacecraftState state, final double[][] dY1dY0,
  99.                                     final double[][] dY1dP, final double[] p) {

  100.         // map the converted state Jacobian to one-dimensional array
  101.         int index = 0;
  102.         for (int i = 0; i < STATE_DIMENSION; ++i) {
  103.             for (int j = 0; j < STATE_DIMENSION; ++j) {
  104.                 p[index++] = (i == j) ? 1.0 : 0.0;
  105.             }
  106.         }

  107.         if (parameters.getNbParams() != 0) {

  108.             // map the converted parameters Jacobian to one-dimensional array
  109.             for (int i = 0; i < STATE_DIMENSION; ++i) {
  110.                 for (int j = 0; j < parameters.getNbParams(); ++j) {
  111.                     p[index++] = dY1dP[i][j];
  112.                 }
  113.             }
  114.         }

  115.     }

  116.     /** {@inheritDoc} */
  117.     public void getStateJacobian(final SpacecraftState state, final double[][] dYdY0) {

  118.         // extract additional state
  119.         final double[] p = state.getAdditionalState(name);

  120.         for (int i = 0; i < STATE_DIMENSION; i++) {
  121.             final double[] row = dYdY0[i];
  122.             for (int j = 0; j < STATE_DIMENSION; j++) {
  123.                 row[j] = p[i * STATE_DIMENSION + j] + shortPeriodDerivatives[i * STATE_DIMENSION + j];
  124.             }
  125.         }

  126.     }


  127.     /** {@inheritDoc} */
  128.     public void getParametersJacobian(final SpacecraftState state, final double[][] dYdP) {

  129.         if (parameters.getNbParams() != 0) {

  130.             // extract the additional state
  131.             final double[] p = state.getAdditionalState(name);

  132.             for (int i = 0; i < STATE_DIMENSION; i++) {
  133.                 final double[] row = dYdP[i];
  134.                 for (int j = 0; j < parameters.getNbParams(); j++) {
  135.                     row[j] = p[STATE_DIMENSION * STATE_DIMENSION + (j + parameters.getNbParams() * i)] +
  136.                              shortPeriodDerivatives[STATE_DIMENSION * STATE_DIMENSION + (j + parameters.getNbParams() * i)];
  137.                 }
  138.             }

  139.         }

  140.     }

  141.     /** Compute the derivatives of the short period terms related to the additional state parameters.
  142.      * @param s Current state information: date, kinematics, attitude, and additional state
  143.      * @deprecated as of 11.1, replaced by {@link #setReferenceState(SpacecraftState)}
  144.      */
  145.     @Deprecated
  146.     public void setShortPeriodJacobians(final SpacecraftState s) {
  147.         setReferenceState(s);
  148.     }

  149.     /** {@inheritDoc} */
  150.     @SuppressWarnings("unchecked")
  151.     @Override
  152.     public void setReferenceState(final SpacecraftState reference) {

  153.         final double[] p = reference.getAdditionalState(name);
  154.         if (shortPeriodDerivatives == null) {
  155.             shortPeriodDerivatives = new double[p.length];
  156.         }

  157.         switch (propagationType) {
  158.             case MEAN :
  159.                 break;
  160.             case OSCULATING :
  161.                 // initialize Jacobians to zero
  162.                 final int paramDim = parameters.getNbParams();
  163.                 final int dim = 6;
  164.                 final double[][] dShortPerioddState = new double[dim][dim];
  165.                 final double[][] dShortPerioddParam = new double[dim][paramDim];
  166.                 final DSSTGradientConverter converter = new DSSTGradientConverter(reference, propagator.getAttitudeProvider());

  167.                 // Compute Jacobian
  168.                 for (final DSSTForceModel forceModel : propagator.getAllForceModels()) {

  169.                     final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
  170.                     final Gradient[] dsParameters = converter.getParameters(dsState, forceModel);
  171.                     final FieldAuxiliaryElements<Gradient> auxiliaryElements = new FieldAuxiliaryElements<>(dsState.getOrbit(), I);

  172.                     final Gradient zero = dsState.getDate().getField().getZero();
  173.                     final List<FieldShortPeriodTerms<Gradient>> shortPeriodTerms = new ArrayList<>();
  174.                     shortPeriodTerms.addAll(forceModel.initializeShortPeriodTerms(auxiliaryElements, propagationType, dsParameters));
  175.                     forceModel.updateShortPeriodTerms(dsParameters, dsState);
  176.                     final Gradient[] shortPeriod = new Gradient[6];
  177.                     Arrays.fill(shortPeriod, zero);
  178.                     for (final FieldShortPeriodTerms<Gradient> spt : shortPeriodTerms) {
  179.                         final Gradient[] spVariation = spt.value(dsState.getOrbit());
  180.                         for (int i = 0; i < spVariation .length; i++) {
  181.                             shortPeriod[i] = shortPeriod[i].add(spVariation[i]);
  182.                         }
  183.                     }

  184.                     final double[] derivativesASP  = shortPeriod[0].getGradient();
  185.                     final double[] derivativesExSP = shortPeriod[1].getGradient();
  186.                     final double[] derivativesEySP = shortPeriod[2].getGradient();
  187.                     final double[] derivativesHxSP = shortPeriod[3].getGradient();
  188.                     final double[] derivativesHySP = shortPeriod[4].getGradient();
  189.                     final double[] derivativesLSP  = shortPeriod[5].getGradient();

  190.                     // update Jacobian with respect to state
  191.                     addToRow(derivativesASP,  0, dShortPerioddState);
  192.                     addToRow(derivativesExSP, 1, dShortPerioddState);
  193.                     addToRow(derivativesEySP, 2, dShortPerioddState);
  194.                     addToRow(derivativesHxSP, 3, dShortPerioddState);
  195.                     addToRow(derivativesHySP, 4, dShortPerioddState);
  196.                     addToRow(derivativesLSP,  5, dShortPerioddState);

  197.                     int index = converter.getFreeStateParameters();
  198.                     for (ParameterDriver driver : forceModel.getParametersDrivers()) {
  199.                         if (driver.isSelected()) {
  200.                             final int parameterIndex = map.get(driver);
  201.                             dShortPerioddParam[0][parameterIndex] += derivativesASP[index];
  202.                             dShortPerioddParam[1][parameterIndex] += derivativesExSP[index];
  203.                             dShortPerioddParam[2][parameterIndex] += derivativesEySP[index];
  204.                             dShortPerioddParam[3][parameterIndex] += derivativesHxSP[index];
  205.                             dShortPerioddParam[4][parameterIndex] += derivativesHySP[index];
  206.                             dShortPerioddParam[5][parameterIndex] += derivativesLSP[index];
  207.                             ++index;
  208.                         }
  209.                     }
  210.                 }

  211.                 // Get orbital short period derivatives with respect orbital elements.
  212.                 for (int i = 0; i < dim; i++) {
  213.                     for (int j = 0; j < dim; j++) {
  214.                         shortPeriodDerivatives[j + dim * i] = dShortPerioddState[i][j];
  215.                     }
  216.                 }

  217.                 // Get orbital short period derivatives with respect to model parameters.
  218.                 final int columnTop = dim * dim;
  219.                 for (int k = 0; k < paramDim; k++) {
  220.                     for (int i = 0; i < dim; ++i) {
  221.                         shortPeriodDerivatives[columnTop + (i + dim * k)] = dShortPerioddParam[i][k];
  222.                     }
  223.                 }
  224.                 break;
  225.             default:
  226.                 throw new OrekitInternalError(null);
  227.         }
  228.     }

  229.     /** Fill Jacobians rows.
  230.      * @param derivatives derivatives of a component
  231.      * @param index component index (0 for a, 1 for ex, 2 for ey, 3 for hx, 4 for hy, 5 for l)
  232.      * @param dMeanElementRatedElement Jacobian of mean elements rate with respect to mean elements
  233.      */
  234.     private void addToRow(final double[] derivatives, final int index,
  235.                           final double[][] dMeanElementRatedElement) {

  236.         for (int i = 0; i < 6; i++) {
  237.             dMeanElementRatedElement[index][i] += derivatives[i];
  238.         }

  239.     }

  240. }