DSSTHarvester.java

  1. /* Copyright 2002-2025 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[getStateDimension()][getStateDimension()];
  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.         final int stateDimension = getStateDimension();
  116.         if (propagator.getPropagationType() == PropagationType.OSCULATING) {
  117.             // add the short period terms
  118.             for (int i = 0; i < stateDimension; i++) {
  119.                 for (int j = 0; j < stateDimension; j++) {
  120.                     stm.addToEntry(i, j, shortPeriodDerivativesStm[i][j]);
  121.                 }
  122.             }
  123.         }

  125.         return stm;

  127.     }

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

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

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

  145.         }

  147.         return jacobian;

  149.     }

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

  160.         // Initialize B1
  161.         final int stateDimension = getStateDimension();
  162.         final RealMatrix B1 = MatrixUtils.createRealMatrix(stateDimension, stateDimension);

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

  171.         // Return B1
  172.         return B1;

  174.     }

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

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

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

  209.         // Initialize B4
  210.         final List<String> names = getJacobiansColumnsNames();
  211.         final RealMatrix B4 = MatrixUtils.createRealMatrix(getStateDimension(), names.size());

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

  221.         // Return B4
  222.         return B4;

  224.     }

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

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

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

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

  257.         // Converter
  258.         final DSSTGradientConverter converter = new DSSTGradientConverter(reference, propagator.getAttitudeProvider());

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

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

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

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

  285.         }

  287.     }

  289.     /** Update the short periodic terms for the "field" elements.
  290.      * @param reference current mean spacecraft state
  291.      */
  292.     @SuppressWarnings("unchecked")
  293.     public void updateFieldShortPeriodTerms(final SpacecraftState reference) {

  295.         // Converter
  296.         final DSSTGradientConverter converter = new DSSTGradientConverter(reference, propagator.getAttitudeProvider());

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

  301.             // Convert to Gradient
  302.             final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
  303.             final Gradient[] dsParameters = converter.getParameters(dsState, forceModel);

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

  308.         }

  310.     }

  312.     /** {@inheritDoc} */
  313.     @Override
  314.     public void setReferenceState(final SpacecraftState reference) {

  316.         // reset derivatives to zero
  317.         for (final double[] row : shortPeriodDerivativesStm) {
  318.             Arrays.fill(row, 0.0);
  319.         }

  321.         shortPeriodDerivativesJacobianColumns.clear();

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

  325.         // Compute Jacobian
  326.         for (final DSSTForceModel forceModel : propagator.getAllForceModels()) {

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

  343.             final double[] derivativesASP  = shortPeriod[0].getGradient();
  344.             final double[] derivativesExSP = shortPeriod[1].getGradient();
  345.             final double[] derivativesEySP = shortPeriod[2].getGradient();
  346.             final double[] derivativesHxSP = shortPeriod[3].getGradient();
  347.             final double[] derivativesHySP = shortPeriod[4].getGradient();
  348.             final double[] derivativesLSP  = shortPeriod[5].getGradient();

  350.             // update Jacobian with respect to state
  351.             addToRow(derivativesASP,  0);
  352.             addToRow(derivativesExSP, 1);
  353.             addToRow(derivativesEySP, 2);
  354.             addToRow(derivativesHxSP, 3);
  355.             addToRow(derivativesHySP, 4);
  356.             addToRow(derivativesLSP,  5);

  358.             int paramsIndex = converter.getFreeStateParameters();
  359.             for (ParameterDriver driver : forceModel.getParametersDrivers()) {
  360.                 if (driver.isSelected()) {

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

  365.                     for (Span<String> span = driverNameSpanMap.getFirstSpan(); span != null; span = span.next()) {
  366.                         // get the partials derivatives for this driver
  367.                         DoubleArrayDictionary.Entry entry = shortPeriodDerivativesJacobianColumns.getEntry(span.getData());
  368.                         if (entry == null) {
  369.                             // create an entry filled with zeroes
  370.                             shortPeriodDerivativesJacobianColumns.put(span.getData(), new double[getStateDimension()]);
  371.                             entry = shortPeriodDerivativesJacobianColumns.getEntry(span.getData());
  372.                         }

  374.                         // add the contribution of the current force model
  375.                         entry.increment(new double[] {
  376.                             derivativesASP[paramsIndex], derivativesExSP[paramsIndex], derivativesEySP[paramsIndex],
  377.                             derivativesHxSP[paramsIndex], derivativesHySP[paramsIndex], derivativesLSP[paramsIndex]
  378.                         });
  379.                         ++paramsIndex;
  380.                     }
  381.                 }
  382.             }
  383.         }

  385.     }

  387.     /** Fill State Transition Matrix rows.
  388.      * @param derivatives derivatives of a component
  389.      * @param index component index (0 for a, 1 for ex, 2 for ey, 3 for hx, 4 for hy, 5 for l)
  390.      */
  391.     private void addToRow(final double[] derivatives, final int index) {
  392.         for (int i = 0; i < 6; i++) {
  393.             shortPeriodDerivativesStm[index][i] += derivatives[i];
  394.         }
  395.     }

  397.     /** {@inheritDoc} */
  398.     @Override
  399.     public OrbitType getOrbitType() {
  400.         return propagator.getOrbitType();
  401.     }

  403.     /** {@inheritDoc} */
  404.     @Override
  405.     public PositionAngleType getPositionAngleType() {
  406.         return propagator.getPositionAngleType();
  407.     }

  409. }
Created with JaCoCo 0.8.12.202403310830