DSSTHarvester.java

  1. /* Copyright 2002-2024 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.IdentityHashMap;
  22. import java.util.List;
  23. import java.util.Map;

  25. import org.hipparchus.analysis.differentiation.Gradient;
  26. import org.hipparchus.linear.MatrixUtils;
  27. import org.hipparchus.linear.RealMatrix;
  28. import org.orekit.orbits.OrbitType;
  29. import org.orekit.orbits.PositionAngleType;
  30. import org.orekit.propagation.AbstractMatricesHarvester;
  31. import org.orekit.propagation.FieldSpacecraftState;
  32. import org.orekit.propagation.PropagationType;
  33. import org.orekit.propagation.SpacecraftState;
  34. import org.orekit.propagation.semianalytical.dsst.forces.DSSTForceModel;
  35. import org.orekit.propagation.semianalytical.dsst.forces.FieldShortPeriodTerms;
  36. import org.orekit.propagation.semianalytical.dsst.utilities.FieldAuxiliaryElements;
  37. import org.orekit.utils.DoubleArrayDictionary;
  38. import org.orekit.utils.ParameterDriver;
  39. import org.orekit.utils.TimeSpanMap;
  40. import org.orekit.utils.TimeSpanMap.Span;

  42. /** Harvester between two-dimensional Jacobian matrices and one-dimensional {@link
  43.  * SpacecraftState#getAdditionalState(String) additional state arrays}.
  44.  * @author Luc Maisonobe
  45.  * @author Bryan Cazabonne
  46.  * @since 11.1
  47.  */
  48. public class DSSTHarvester extends AbstractMatricesHarvester {

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

  65.     /** Propagator bound to this harvester. */
  66.     private final DSSTPropagator propagator;

  68.     /** Derivatives of the short period terms that apply to State Transition Matrix.*/
  69.     private final double[][] shortPeriodDerivativesStm;

  71.     /** Derivatives of the short period terms that apply to Jacobians columns. */
  72.     private final DoubleArrayDictionary shortPeriodDerivativesJacobianColumns;

  74.     /** Columns names for parameters. */
  75.     private List<String> columnsNames;

  77.     /**
  78.      * Field short periodic terms. Key is the force model to which they pertain. Value is
  79.      * the terms. They need to be stored in a map because the DsstForceModel interface
  80.      * does not have a getter for the terms.
  81.      */
  82.     private final Map<DSSTForceModel, List<FieldShortPeriodTerms<Gradient>>>
  83.             fieldShortPeriodTerms;

  85.     /** Simple constructor.
  86.      * <p>
  87.      * The arguments for initial matrices <em>must</em> be compatible with the
  88.      * {@link org.orekit.orbits.OrbitType#EQUINOCTIAL equinoctial orbit type}
  89.      * and {@link PositionAngleType position angle} that will be used by propagator
  90.      * </p>
  91.      * @param propagator propagator bound to this harvester
  92.      * @param stmName State Transition Matrix state name
  93.      * @param initialStm initial State Transition Matrix ∂Y/∂Y₀,
  94.      * if null (which is the most frequent case), assumed to be 6x6 identity
  95.      * @param initialJacobianColumns initial columns of the Jacobians matrix with respect to parameters,
  96.      * if null or if some selected parameters are missing from the dictionary, the corresponding
  97.      * initial column is assumed to be 0
  98.      */
  99.     DSSTHarvester(final DSSTPropagator propagator, final String stmName,
  100.                   final RealMatrix initialStm, final DoubleArrayDictionary initialJacobianColumns) {
  101.         super(stmName, initialStm, initialJacobianColumns);
  102.         this.propagator                            = propagator;
  103.         this.shortPeriodDerivativesStm             = new double[STATE_DIMENSION][STATE_DIMENSION];
  104.         this.shortPeriodDerivativesJacobianColumns = new DoubleArrayDictionary();
  105.         // Use identity hash map to have the same behavior as a getter on the force model
  106.         this.fieldShortPeriodTerms                 = new IdentityHashMap<>();
  107.     }

  109.     /** {@inheritDoc} */
  110.     @Override
  111.     public RealMatrix getStateTransitionMatrix(final SpacecraftState state) {

  113.         final RealMatrix stm = super.getStateTransitionMatrix(state);

  115.         if (propagator.getPropagationType() == PropagationType.OSCULATING) {
  116.             // add the short period terms
  117.             for (int i = 0; i < STATE_DIMENSION; i++) {
  118.                 for (int j = 0; j < STATE_DIMENSION; j++) {
  119.                     stm.addToEntry(i, j, shortPeriodDerivativesStm[i][j]);
  120.                 }
  121.             }
  122.         }

  124.         return stm;

  126.     }

  128.     /** {@inheritDoc} */
  129.     @Override
  130.     public RealMatrix getParametersJacobian(final SpacecraftState state) {

  132.         final RealMatrix jacobian = super.getParametersJacobian(state);
  133.         if (jacobian != null && propagator.getPropagationType() == PropagationType.OSCULATING) {

  135.             // add the short period terms
  136.             final List<String> names = getJacobiansColumnsNames();
  137.             for (int j = 0; j < names.size(); ++j) {
  138.                 final double[] column = shortPeriodDerivativesJacobianColumns.get(names.get(j));
  139.                 for (int i = 0; i < STATE_DIMENSION; i++) {
  140.                     jacobian.addToEntry(i, j, column[i]);
  141.                 }
  142.             }

  144.         }

  146.         return jacobian;

  148.     }

  150.     /** Get the Jacobian matrix B1 (B1 = ∂εη/∂Y).
  151.      * <p>
  152.      * B1 represents the partial derivatives of the short period motion
  153.      * with respect to the mean equinoctial elements.
  154.      * </p>
  155.      * @return the B1 jacobian matrix
  156.      */
  157.     public RealMatrix getB1() {

  159.         // Initialize B1
  160.         final RealMatrix B1 = MatrixUtils.createRealMatrix(STATE_DIMENSION, STATE_DIMENSION);

  162.         // add the short period terms
  163.         for (int i = 0; i < STATE_DIMENSION; i++) {
  164.             for (int j = 0; j < STATE_DIMENSION; j++) {
  165.                 B1.addToEntry(i, j, shortPeriodDerivativesStm[i][j]);
  166.             }
  167.         }

  169.         // Return B1
  170.         return B1;

  172.     }

  174.     /** Get the Jacobian matrix B2 (B2 = ∂Y/∂Y₀).
  175.      * <p>
  176.      * B2 represents the partial derivatives of the mean equinoctial elements
  177.      * with respect to the initial ones.
  178.      * </p>
  179.      * @param state spacecraft state
  180.      * @return the B2 jacobian matrix
  181.      */
  182.     public RealMatrix getB2(final SpacecraftState state) {
  183.         return super.getStateTransitionMatrix(state);
  184.     }

  186.     /** Get the Jacobian matrix B3 (B3 = ∂Y/∂P).
  187.      * <p>
  188.      * B3 represents the partial derivatives of the mean equinoctial elements
  189.      * with respect to the estimated propagation parameters.
  190.      * </p>
  191.      * @param state spacecraft state
  192.      * @return the B3 jacobian matrix
  193.      */
  194.     public RealMatrix getB3(final SpacecraftState state) {
  195.         return super.getParametersJacobian(state);
  196.     }

  198.     /** Get the Jacobian matrix B4 (B4 = ∂εη/∂c).
  199.      * <p>
  200.      * B4 represents the partial derivatives of the short period motion
  201.      * with respect to the estimated propagation parameters.
  202.      * </p>
  203.      * @return the B4 jacobian matrix
  204.      */
  205.     public RealMatrix getB4() {

  207.         // Initialize B4
  208.         final List<String> names = getJacobiansColumnsNames();
  209.         final RealMatrix B4 = MatrixUtils.createRealMatrix(STATE_DIMENSION, names.size());

  211.         // add the short period terms
  212.         for (int j = 0; j < names.size(); ++j) {
  213.             final double[] column = shortPeriodDerivativesJacobianColumns.get(names.get(j));
  214.             for (int i = 0; i < STATE_DIMENSION; i++) {
  215.                 B4.addToEntry(i, j, column[i]);
  216.             }
  217.         }

  219.         // Return B4
  220.         return B4;

  222.     }

  224.     /** Freeze the names of the Jacobian columns.
  225.      * <p>
  226.      * This method is called when proagation starts, i.e. when configuration is completed
  227.      * </p>
  228.      */
  229.     public void freezeColumnsNames() {
  230.         columnsNames = getJacobiansColumnsNames();
  231.     }

  233.     /** {@inheritDoc} */
  234.     @Override
  235.     public List<String> getJacobiansColumnsNames() {
  236.         return columnsNames == null ? propagator.getJacobiansColumnsNames() : columnsNames;
  237.     }

  239.     /** Initialize the short periodic terms for the "field" elements.
  240.      * @param reference current mean spacecraft state
  241.      */
  242.     public void initializeFieldShortPeriodTerms(final SpacecraftState reference) {
  243.         initializeFieldShortPeriodTerms(reference, propagator.getPropagationType());
  244.     }

  246.     /**
  247.      * Initialize the short periodic terms for the "field" elements.
  248.      *
  249.      * @param reference current mean spacecraft state
  250.      * @param type      MEAN or OSCULATING
  251.      */
  252.     public void initializeFieldShortPeriodTerms(final SpacecraftState reference,
  253.                                                 final PropagationType type) {

  255.         // Converter
  256.         final DSSTGradientConverter converter = new DSSTGradientConverter(reference, propagator.getAttitudeProvider());

  258.         // clear old values
  259.         // prevents duplicates or stale values when reusing a DSSTPropagator
  260.         fieldShortPeriodTerms.clear();

  262.         // Loop on force models
  263.         for (final DSSTForceModel forceModel : propagator.getAllForceModels()) {

  265.             // Convert to Gradient
  266.             final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
  267.             final Gradient[] dsParameters = converter.getParametersAtStateDate(dsState, forceModel);
  268.             final FieldAuxiliaryElements<Gradient> auxiliaryElements = new FieldAuxiliaryElements<>(dsState.getOrbit(), I);

  270.             // Initialize the "Field" short periodic terms, same mode as the propagator
  271.             final List<FieldShortPeriodTerms<Gradient>> terms =
  272.                     forceModel.initializeShortPeriodTerms(
  273.                             auxiliaryElements,
  274.                             type,
  275.                             dsParameters);
  276.             // create a copy of the list to protect against inadvertent modification
  277.             fieldShortPeriodTerms.computeIfAbsent(forceModel, x -> new ArrayList<>())
  278.                     .addAll(terms);

  280.         }

  282.     }

  284.     /** Update the short periodic terms for the "field" elements.
  285.      * @param reference current mean spacecraft state
  286.      */
  287.     @SuppressWarnings("unchecked")
  288.     public void updateFieldShortPeriodTerms(final SpacecraftState reference) {

  290.         // Converter
  291.         final DSSTGradientConverter converter = new DSSTGradientConverter(reference, propagator.getAttitudeProvider());

  293.         // Loop on force models
  294.         for (final DSSTForceModel forceModel : propagator.getAllForceModels()) {

  296.             // Convert to Gradient
  297.             final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
  298.             final Gradient[] dsParameters = converter.getParameters(dsState, forceModel);

  300.             // Update the short periodic terms for the current force model
  301.             forceModel.updateShortPeriodTerms(dsParameters, dsState);

  303.         }

  305.     }

  307.     /** {@inheritDoc} */
  308.     @Override
  309.     public void setReferenceState(final SpacecraftState reference) {

  311.         // reset derivatives to zero
  312.         for (final double[] row : shortPeriodDerivativesStm) {
  313.             Arrays.fill(row, 0.0);
  314.         }

  316.         shortPeriodDerivativesJacobianColumns.clear();

  318.         final DSSTGradientConverter converter = new DSSTGradientConverter(reference, propagator.getAttitudeProvider());

  320.         // Compute Jacobian
  321.         for (final DSSTForceModel forceModel : propagator.getAllForceModels()) {

  323.             final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
  324.             final Gradient zero = dsState.getDate().getField().getZero();
  325.             final Gradient[] shortPeriod = new Gradient[6];
  326.             Arrays.fill(shortPeriod, zero);
  327.             final List<FieldShortPeriodTerms<Gradient>> terms = fieldShortPeriodTerms
  328.                     .computeIfAbsent(forceModel, x -> new ArrayList<>(0));
  329.             for (final FieldShortPeriodTerms<Gradient> spt : terms) {
  330.                 final Gradient[] spVariation = spt.value(dsState.getOrbit());
  331.                 for (int i = 0; i < spVariation .length; i++) {
  332.                     shortPeriod[i] = shortPeriod[i].add(spVariation[i]);
  333.                 }
  334.             }

  336.             final double[] derivativesASP  = shortPeriod[0].getGradient();
  337.             final double[] derivativesExSP = shortPeriod[1].getGradient();
  338.             final double[] derivativesEySP = shortPeriod[2].getGradient();
  339.             final double[] derivativesHxSP = shortPeriod[3].getGradient();
  340.             final double[] derivativesHySP = shortPeriod[4].getGradient();
  341.             final double[] derivativesLSP  = shortPeriod[5].getGradient();

  343.             // update Jacobian with respect to state
  344.             addToRow(derivativesASP,  0);
  345.             addToRow(derivativesExSP, 1);
  346.             addToRow(derivativesEySP, 2);
  347.             addToRow(derivativesHxSP, 3);
  348.             addToRow(derivativesHySP, 4);
  349.             addToRow(derivativesLSP,  5);

  351.             int paramsIndex = converter.getFreeStateParameters();
  352.             for (ParameterDriver driver : forceModel.getParametersDrivers()) {
  353.                 if (driver.isSelected()) {

  355.                     final TimeSpanMap<String> driverNameSpanMap = driver.getNamesSpanMap();
  356.                     // for each span (for each estimated value) corresponding name is added

  358.                     for (Span<String> span = driverNameSpanMap.getFirstSpan(); span != null; span = span.next()) {
  359.                         // get the partials derivatives for this driver
  360.                         DoubleArrayDictionary.Entry entry = shortPeriodDerivativesJacobianColumns.getEntry(span.getData());
  361.                         if (entry == null) {
  362.                             // create an entry filled with zeroes
  363.                             shortPeriodDerivativesJacobianColumns.put(span.getData(), new double[STATE_DIMENSION]);
  364.                             entry = shortPeriodDerivativesJacobianColumns.getEntry(span.getData());
  365.                         }

  367.                         // add the contribution of the current force model
  368.                         entry.increment(new double[] {
  369.                             derivativesASP[paramsIndex], derivativesExSP[paramsIndex], derivativesEySP[paramsIndex],
  370.                             derivativesHxSP[paramsIndex], derivativesHySP[paramsIndex], derivativesLSP[paramsIndex]
  371.                         });
  372.                         ++paramsIndex;
  373.                     }
  374.                 }
  375.             }
  376.         }

  378.     }

  380.     /** Fill State Transition Matrix rows.
  381.      * @param derivatives derivatives of a component
  382.      * @param index component index (0 for a, 1 for ex, 2 for ey, 3 for hx, 4 for hy, 5 for l)
  383.      */
  384.     private void addToRow(final double[] derivatives, final int index) {
  385.         for (int i = 0; i < 6; i++) {
  386.             shortPeriodDerivativesStm[index][i] += derivatives[i];
  387.         }
  388.     }

  390.     /** {@inheritDoc} */
  391.     @Override
  392.     public OrbitType getOrbitType() {
  393.         return propagator.getOrbitType();
  394.     }

  396.     /** {@inheritDoc} */
  397.     @Override
  398.     public PositionAngleType getPositionAngleType() {
  399.         return propagator.getPositionAngleType();
  400.     }

  402. }
Created with JaCoCo 0.8.12.202403310830