DSSTJacobiansMapper.java

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

  19. import java.util.ArrayList;
  20. import java.util.Arrays;
  21. import java.util.List;
  22. import java.util.Map;

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

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

  51.     /** State dimension, fixed to 6.
  52.      * @since 9.0
  53.      */
  54.     public static final int STATE_DIMENSION = 6;

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

  71.     /** Name. */
  72.     private String name;

  74.     /** Selected parameters for Jacobian computation. */
  75.     private final ParameterDriversList parameters;

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

  80.     /** Propagator computing state evolution. */
  81.     private final DSSTPropagator propagator;

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

  86.     /** Type of the orbit used for the propagation.*/
  87.     private PropagationType propagationType;

  89.     /** Simple constructor.
  90.      * @param name name of the Jacobians
  91.      * @param parameters selected parameters for Jacobian computation
  92.      * @param propagator the propagator that will handle the orbit propagation
  93.      * @param map parameters map
  94.      * @param propagationType type of the orbit used for the propagation (mean or osculating)
  95.      */
  96.     DSSTJacobiansMapper(final String name,
  97.                         final ParameterDriversList parameters,
  98.                         final DSSTPropagator propagator,
  99.                         final Map<ParameterDriver, Integer> map,
  100.                         final PropagationType propagationType) {

  102.         super(name, parameters);

  104.         shortPeriodDerivatives = null;

  106.         this.parameters      = parameters;
  107.         this.name            = name;
  108.         this.propagator      = propagator;
  109.         this.map             = map;
  110.         this.propagationType = propagationType;

  112.     }

  114.     /** {@inheritDoc} */
  115.     protected double[][] getConversionJacobian(final SpacecraftState state) {

  117.         final double[][] identity = new double[STATE_DIMENSION][STATE_DIMENSION];

  119.         for (int i = 0; i < STATE_DIMENSION; ++i) {
  120.             identity[i][i] = 1.0;
  121.         }

  123.         return identity;

  125.     }

  127.     /** {@inheritDoc} */
  128.     public void setInitialJacobians(final SpacecraftState state, final double[][] dY1dY0,
  129.                                     final double[][] dY1dP, final double[] p) {

  131.         // map the converted state Jacobian to one-dimensional array
  132.         int index = 0;
  133.         for (int i = 0; i < STATE_DIMENSION; ++i) {
  134.             for (int j = 0; j < STATE_DIMENSION; ++j) {
  135.                 p[index++] = (i == j) ? 1.0 : 0.0;
  136.             }
  137.         }

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

  141.             // map the converted parameters Jacobian to one-dimensional array
  142.             for (int i = 0; i < STATE_DIMENSION; ++i) {
  143.                 for (int j = 0; j < parameters.getNbParams(); ++j) {
  144.                     p[index++] = dY1dP[i][j];
  145.                 }
  146.             }
  147.         }

  149.     }

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

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

  157.         for (int i = 0; i < STATE_DIMENSION; i++) {
  158.             final double[] row = dYdY0[i];
  159.             for (int j = 0; j < STATE_DIMENSION; j++) {
  160.                 row[j] = p[i * STATE_DIMENSION + j] + shortPeriodDerivatives[i * STATE_DIMENSION + j];
  161.             }
  162.         }

  164.     }


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

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

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

  175.             for (int i = 0; i < STATE_DIMENSION; i++) {
  176.                 final double[] row = dYdP[i];
  177.                 for (int j = 0; j < parameters.getNbParams(); j++) {
  178.                     row[j] = p[STATE_DIMENSION * STATE_DIMENSION + (j + parameters.getNbParams() * i)] +
  179.                              shortPeriodDerivatives[STATE_DIMENSION * STATE_DIMENSION + (j + parameters.getNbParams() * i)];
  180.                 }
  181.             }

  183.         }

  185.     }

  187.     /** {@inheritDoc} */
  188.     @Override
  189.     public int getAdditionalStateDimension() {
  190.         return STATE_DIMENSION * (STATE_DIMENSION + parameters.getNbParams());
  191.     }

  193.     /** Compute the derivatives of the short period terms related to the additional state parameters.
  194.     * @param s Current state information: date, kinematics, attitude, and additional state
  195.     */
  196.     @SuppressWarnings("unchecked")
  197.     public void setShortPeriodJacobians(final SpacecraftState s) {

  199.         final double[] p = s.getAdditionalState(name);
  200.         if (shortPeriodDerivatives == null) {
  201.             shortPeriodDerivatives = new double[p.length];
  202.         }

  204.         switch (propagationType) {
  205.             case MEAN :
  206.                 break;
  207.             case OSCULATING :
  208.                 // initialize Jacobians to zero
  209.                 final int paramDim = parameters.getNbParams();
  210.                 final int dim = 6;
  211.                 final double[][] dShortPerioddState = new double[dim][dim];
  212.                 final double[][] dShortPerioddParam = new double[dim][paramDim];
  213.                 final DSSTGradientConverter converter = new DSSTGradientConverter(s, propagator.getAttitudeProvider());

  215.                 // Compute Jacobian
  216.                 for (final DSSTForceModel forceModel : propagator.getAllForceModels()) {

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

  222.                     final Gradient zero = dsState.getDate().getField().getZero();
  223.                     final List<FieldShortPeriodTerms<Gradient>> shortPeriodTerms = new ArrayList<>();
  224.                     shortPeriodTerms.addAll(forceModel.initialize(auxiliaryElements, propagationType, dsParameters));
  225.                     forceModel.updateShortPeriodTerms(dsParameters, dsState);
  226.                     final Gradient[] shortPeriod = new Gradient[6];
  227.                     Arrays.fill(shortPeriod, zero);
  228.                     for (final FieldShortPeriodTerms<Gradient> spt : shortPeriodTerms) {
  229.                         final Gradient[] spVariation = spt.value(dsState.getOrbit());
  230.                         for (int i = 0; i < spVariation .length; i++) {
  231.                             shortPeriod[i] = shortPeriod[i].add(spVariation[i]);
  232.                         }
  233.                     }

  235.                     final double[] derivativesASP  = shortPeriod[0].getGradient();
  236.                     final double[] derivativesExSP = shortPeriod[1].getGradient();
  237.                     final double[] derivativesEySP = shortPeriod[2].getGradient();
  238.                     final double[] derivativesHxSP = shortPeriod[3].getGradient();
  239.                     final double[] derivativesHySP = shortPeriod[4].getGradient();
  240.                     final double[] derivativesLSP  = shortPeriod[5].getGradient();

  242.                     // update Jacobian with respect to state
  243.                     addToRow(derivativesASP,  0, dShortPerioddState);
  244.                     addToRow(derivativesExSP, 1, dShortPerioddState);
  245.                     addToRow(derivativesEySP, 2, dShortPerioddState);
  246.                     addToRow(derivativesHxSP, 3, dShortPerioddState);
  247.                     addToRow(derivativesHySP, 4, dShortPerioddState);
  248.                     addToRow(derivativesLSP,  5, dShortPerioddState);

  250.                     int index = converter.getFreeStateParameters();
  251.                     for (ParameterDriver driver : forceModel.getParametersDrivers()) {
  252.                         if (driver.isSelected()) {
  253.                             final int parameterIndex = map.get(driver);
  254.                             dShortPerioddParam[0][parameterIndex] += derivativesASP[index];
  255.                             dShortPerioddParam[1][parameterIndex] += derivativesExSP[index];
  256.                             dShortPerioddParam[2][parameterIndex] += derivativesEySP[index];
  257.                             dShortPerioddParam[3][parameterIndex] += derivativesHxSP[index];
  258.                             dShortPerioddParam[4][parameterIndex] += derivativesHySP[index];
  259.                             dShortPerioddParam[5][parameterIndex] += derivativesLSP[index];
  260.                             ++index;
  261.                         }
  262.                     }
  263.                 }

  265.                 // Get orbital short period derivatives with respect orbital elements.
  266.                 for (int i = 0; i < dim; i++) {
  267.                     for (int j = 0; j < dim; j++) {
  268.                         shortPeriodDerivatives[j + dim * i] = dShortPerioddState[i][j];
  269.                     }
  270.                 }

  272.                 // Get orbital short period derivatives with respect to model parameters.
  273.                 final int columnTop = dim * dim;
  274.                 for (int k = 0; k < paramDim; k++) {
  275.                     for (int i = 0; i < dim; ++i) {
  276.                         shortPeriodDerivatives[columnTop + (i + dim * k)] = dShortPerioddParam[i][k];
  277.                     }
  278.                 }
  279.                 break;
  280.             default:
  281.                 throw new OrekitInternalError(null);
  282.         }
  283.     }

  285.     /** Fill Jacobians rows.
  286.      * @param derivatives derivatives of a component
  287.      * @param index component index (0 for a, 1 for ex, 2 for ey, 3 for hx, 4 for hy, 5 for l)
  288.      * @param dMeanElementRatedElement Jacobian of mean elements rate with respect to mean elements
  289.      */
  290.     private void addToRow(final double[] derivatives, final int index,
  291.                           final double[][] dMeanElementRatedElement) {

  293.         for (int i = 0; i < 6; i++) {
  294.             dMeanElementRatedElement[index][i] += derivatives[i];
  295.         }

  297.     }

  299. }
Created with JaCoCo 0.8.6.202009150832