DSSTStateTransitionMatrixGenerator.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;

  19. import java.util.HashMap;
  20. import java.util.List;
  21. import java.util.Map;

  23. import org.hipparchus.analysis.differentiation.Gradient;
  24. import org.hipparchus.exception.LocalizedCoreFormats;
  25. import org.hipparchus.linear.MatrixUtils;
  26. import org.hipparchus.linear.RealMatrix;
  27. import org.orekit.attitudes.AttitudeProvider;
  28. import org.orekit.errors.OrekitException;
  29. import org.orekit.propagation.FieldSpacecraftState;
  30. import org.orekit.propagation.SpacecraftState;
  31. import org.orekit.propagation.integration.AdditionalDerivativesProvider;
  32. import org.orekit.propagation.integration.CombinedDerivatives;
  33. import org.orekit.propagation.semianalytical.dsst.forces.DSSTForceModel;
  34. import org.orekit.propagation.semianalytical.dsst.utilities.FieldAuxiliaryElements;
  35. import org.orekit.utils.DoubleArrayDictionary;
  36. import org.orekit.utils.ParameterDriver;

  38. /** Generator for State Transition Matrix.
  39.  * @author Luc Maisonobe
  40.  * @since 11.1
  41.  */
  42. class DSSTStateTransitionMatrixGenerator implements AdditionalDerivativesProvider {

  44.     /** Space dimension. */
  45.     private static final int SPACE_DIMENSION = 3;

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

  62.     /** State dimension. */
  63.     public static final int STATE_DIMENSION = 2 * SPACE_DIMENSION;

  65.     /** Name of the Cartesian STM additional state. */
  66.     private final String stmName;

  68.     /** Force models used in propagation. */
  69.     private final List<DSSTForceModel> forceModels;

  71.     /** Attitude provider used in propagation. */
  72.     private final AttitudeProvider attitudeProvider;

  74.     /** Observers for partial derivatives. */
  75.     private final Map<String, DSSTPartialsObserver> partialsObservers;

  77.     /** Simple constructor.
  78.      * @param stmName name of the Cartesian STM additional state
  79.      * @param forceModels force models used in propagation
  80.      * @param attitudeProvider attitude provider used in propagation
  81.      */
  82.     DSSTStateTransitionMatrixGenerator(final String stmName, final List<DSSTForceModel> forceModels,
  83.                                        final AttitudeProvider attitudeProvider) {
  84.         this.stmName           = stmName;
  85.         this.forceModels       = forceModels;
  86.         this.attitudeProvider  = attitudeProvider;
  87.         this.partialsObservers = new HashMap<>();
  88.     }

  90.     /** Register an observer for partial derivatives.
  91.      * <p>
  92.      * The observer {@link DSSTPartialsObserver#partialsComputed(double[], double[]) partialsComputed}
  93.      * method will be called when partial derivatives are computed, as a side effect of
  94.      * calling {@link #generate(SpacecraftState)}
  95.      * </p>
  96.      * @param name name of the parameter driver this observer is interested in (may be null)
  97.      * @param observer observer to register
  98.      */
  99.     void addObserver(final String name, final DSSTPartialsObserver observer) {
  100.         partialsObservers.put(name, observer);
  101.     }

  103.     /** {@inheritDoc} */
  104.     @Override
  105.     public String getName() {
  106.         return stmName;
  107.     }

  109.     /** {@inheritDoc} */
  110.     @Override
  111.     public int getDimension() {
  112.         return STATE_DIMENSION * STATE_DIMENSION;
  113.     }

  115.     /** {@inheritDoc} */
  116.     @Override
  117.     public boolean yield(final SpacecraftState state) {
  118.         return !state.hasAdditionalState(getName());
  119.     }

  121.     /** Set the initial value of the State Transition Matrix.
  122.      * <p>
  123.      * The returned state must be added to the propagator.
  124.      * </p>
  125.      * @param state initial state
  126.      * @param dYdY0 initial State Transition Matrix ∂Y/∂Y₀,
  127.      * if null (which is the most frequent case), assumed to be 6x6 identity
  128.      * @return state with initial STM (converted to Cartesian ∂C/∂Y₀) added
  129.      */
  130.     SpacecraftState setInitialStateTransitionMatrix(final SpacecraftState state, final RealMatrix dYdY0) {

  132.         if (dYdY0 != null) {
  133.             if (dYdY0.getRowDimension() != STATE_DIMENSION ||
  134.                             dYdY0.getColumnDimension() != STATE_DIMENSION) {
  135.                 throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH_2x2,
  136.                                           dYdY0.getRowDimension(), dYdY0.getColumnDimension(),
  137.                                           STATE_DIMENSION, STATE_DIMENSION);
  138.             }
  139.         }

  141.         // flatten matrix
  142.         final double[] flat = new double[STATE_DIMENSION * STATE_DIMENSION];
  143.         int k = 0;
  144.         for (int i = 0; i < STATE_DIMENSION; ++i) {
  145.             for (int j = 0; j < STATE_DIMENSION; ++j) {
  146.                 flat[k++] = dYdY0.getEntry(i, j);
  147.             }
  148.         }

  150.         // set additional state
  151.         return state.addAdditionalState(stmName, flat);

  153.     }

  155.     /** {@inheritDoc} */
  156.     @Override
  157.     @Deprecated
  158.     public double[] derivatives(final SpacecraftState state) {
  159.         return combinedDerivatives(state).getAdditionalDerivatives();
  160.     }

  162.     /** {@inheritDoc} */
  163.     public CombinedDerivatives combinedDerivatives(final SpacecraftState state) {

  165.         final double[] p = state.getAdditionalState(getName());
  166.         final double[] res = new double[p.length];

  168.         // perform matrix multiplication with matrices flatten
  169.         final RealMatrix factor = computePartials(state);
  170.         int index = 0;
  171.         for (int i = 0; i < STATE_DIMENSION; ++i) {
  172.             for (int j = 0; j < STATE_DIMENSION; ++j) {
  173.                 double sum = 0;
  174.                 for (int k = 0; k < STATE_DIMENSION; ++k) {
  175.                     sum += factor.getEntry(i, k) * p[j + k * STATE_DIMENSION];
  176.                 }
  177.                 res[index++] = sum;
  178.             }
  179.         }

  181.         return new CombinedDerivatives(res, null);

  183.     }

  185.     /** Compute the various partial derivatives.
  186.      * @param state current spacecraft state
  187.      * @return factor matrix
  188.      */
  189.     private RealMatrix computePartials(final SpacecraftState state) {

  191.         // set up containers for partial derivatives
  192.         final RealMatrix            factor               = MatrixUtils.createRealMatrix(STATE_DIMENSION, STATE_DIMENSION);
  193.         final DoubleArrayDictionary meanElementsPartials = new DoubleArrayDictionary();
  194.         final DSSTGradientConverter converter            = new DSSTGradientConverter(state, attitudeProvider);

  196.         // Compute Jacobian
  197.         for (final DSSTForceModel forceModel : forceModels) {

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

  203.             final Gradient[] meanElementRate = forceModel.getMeanElementRate(dsState, auxiliaryElements, parameters);
  204.             final double[] derivativesA  = meanElementRate[0].getGradient();
  205.             final double[] derivativesEx = meanElementRate[1].getGradient();
  206.             final double[] derivativesEy = meanElementRate[2].getGradient();
  207.             final double[] derivativesHx = meanElementRate[3].getGradient();
  208.             final double[] derivativesHy = meanElementRate[4].getGradient();
  209.             final double[] derivativesL  = meanElementRate[5].getGradient();

  211.             // update Jacobian with respect to state
  212.             addToRow(derivativesA,  0, factor);
  213.             addToRow(derivativesEx, 1, factor);
  214.             addToRow(derivativesEy, 2, factor);
  215.             addToRow(derivativesHx, 3, factor);
  216.             addToRow(derivativesHy, 4, factor);
  217.             addToRow(derivativesL,  5, factor);

  219.             // partials derivatives with respect to parameters
  220.             int paramsIndex = converter.getFreeStateParameters();
  221.             for (ParameterDriver driver : forceModel.getParametersDrivers()) {
  222.                 if (driver.isSelected()) {

  224.                     // get the partials derivatives for this driver
  225.                     DoubleArrayDictionary.Entry entry = meanElementsPartials.getEntry(driver.getName());
  226.                     if (entry == null) {
  227.                         // create an entry filled with zeroes
  228.                         meanElementsPartials.put(driver.getName(), new double[STATE_DIMENSION]);
  229.                         entry = meanElementsPartials.getEntry(driver.getName());
  230.                     }

  232.                     // add the contribution of the current force model
  233.                     entry.increment(new double[] {
  234.                         derivativesA[paramsIndex], derivativesEx[paramsIndex], derivativesEy[paramsIndex],
  235.                         derivativesHx[paramsIndex], derivativesHy[paramsIndex], derivativesL[paramsIndex]
  236.                     });
  237.                     ++paramsIndex;

  239.                 }
  240.             }

  242.         }

  244.         // notify observers
  245.         for (Map.Entry<String, DSSTPartialsObserver> observersEntry : partialsObservers.entrySet()) {
  246.             final DoubleArrayDictionary.Entry entry = meanElementsPartials.getEntry(observersEntry.getKey());
  247.             observersEntry.getValue().partialsComputed(state, factor, entry == null ? new double[STATE_DIMENSION] : entry.getValue());
  248.         }

  250.         return factor;

  252.     }

  254.     /** Fill Jacobians rows.
  255.      * @param derivatives derivatives of a component
  256.      * @param index component index (0 for a, 1 for ex, 2 for ey, 3 for hx, 4 for hy, 5 for l)
  257.      * @param factor Jacobian of mean elements rate with respect to mean elements
  258.      */
  259.     private void addToRow(final double[] derivatives, final int index, final RealMatrix factor) {
  260.         for (int i = 0; i < 6; i++) {
  261.             factor.addToEntry(index, i, derivatives[i]);
  262.         }
  263.     }

  265.     /** Interface for observing partials derivatives. */
  266.     public interface DSSTPartialsObserver {

  268.         /** Callback called when partial derivatives have been computed.
  269.          * @param state current spacecraft state
  270.          * @param factor factor matrix
  271.          * @param meanElementsPartials partials derivatives of mean elements rates with respect to the parameter driver
  272.          * that was registered (zero if no parameters were not selected or parameter is unknown)
  273.          */
  274.         void partialsComputed(SpacecraftState state, RealMatrix factor, double[] meanElementsPartials);

  276.     }

  278. }
Created with JaCoCo 0.8.8.202204050719