DSSTHarvester.java

  1. /* Copyright 2002-2023 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 org.hipparchus.analysis.differentiation.Gradient;
  22. import org.hipparchus.linear.MatrixUtils;
  23. import org.hipparchus.linear.RealMatrix;
  24. import org.orekit.orbits.OrbitType;
  25. import org.orekit.orbits.PositionAngleType;
  26. import org.orekit.propagation.AbstractMatricesHarvester;
  27. import org.orekit.propagation.FieldSpacecraftState;
  28. import org.orekit.propagation.PropagationType;
  29. import org.orekit.propagation.SpacecraftState;
  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.DoubleArrayDictionary;
  34. import org.orekit.utils.ParameterDriver;
  35. import org.orekit.utils.TimeSpanMap;
  36. import org.orekit.utils.TimeSpanMap.Span;

  37. /** Harvester between two-dimensional Jacobian matrices and one-dimensional {@link
  38.  * SpacecraftState#getAdditionalState(String) additional state arrays}.
  39.  * @author Luc Maisonobe
  40.  * @author Bryan Cazabonne
  41.  * @since 11.1
  42.  */
  43. public class DSSTHarvester extends AbstractMatricesHarvester {

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

  58.     /** Propagator bound to this harvester. */
  59.     private final DSSTPropagator propagator;

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

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

  64.     /** Columns names for parameters. */
  65.     private List<String> columnsNames;

  66.     /** Field short periodic terms. */
  67.     private List<FieldShortPeriodTerms<Gradient>> fieldShortPeriodTerms;

  68.     /** Simple constructor.
  69.      * <p>
  70.      * The arguments for initial matrices <em>must</em> be compatible with the
  71.      * {@link org.orekit.orbits.OrbitType#EQUINOCTIAL equinoctial orbit type}
  72.      * and {@link PositionAngleType position angle} that will be used by propagator
  73.      * </p>
  74.      * @param propagator propagator bound to this harvester
  75.      * @param stmName State Transition Matrix state name
  76.      * @param initialStm initial State Transition Matrix ∂Y/∂Y₀,
  77.      * if null (which is the most frequent case), assumed to be 6x6 identity
  78.      * @param initialJacobianColumns initial columns of the Jacobians matrix with respect to parameters,
  79.      * if null or if some selected parameters are missing from the dictionary, the corresponding
  80.      * initial column is assumed to be 0
  81.      */
  82.     DSSTHarvester(final DSSTPropagator propagator, final String stmName,
  83.                   final RealMatrix initialStm, final DoubleArrayDictionary initialJacobianColumns) {
  84.         super(stmName, initialStm, initialJacobianColumns);
  85.         this.propagator                            = propagator;
  86.         this.shortPeriodDerivativesStm             = new double[STATE_DIMENSION][STATE_DIMENSION];
  87.         this.shortPeriodDerivativesJacobianColumns = new DoubleArrayDictionary();
  88.         this.fieldShortPeriodTerms                 = new ArrayList<>();
  89.     }

  90.     /** {@inheritDoc} */
  91.     @Override
  92.     public RealMatrix getStateTransitionMatrix(final SpacecraftState state) {

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

  94.         if (propagator.getPropagationType() == PropagationType.OSCULATING) {
  95.             // add the short period terms
  96.             for (int i = 0; i < STATE_DIMENSION; i++) {
  97.                 for (int j = 0; j < STATE_DIMENSION; j++) {
  98.                     stm.addToEntry(i, j, shortPeriodDerivativesStm[i][j]);
  99.                 }
  100.             }
  101.         }

  102.         return stm;

  103.     }

  104.     /** {@inheritDoc} */
  105.     @Override
  106.     public RealMatrix getParametersJacobian(final SpacecraftState state) {

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

  109.             // add the short period terms
  110.             final List<String> names = getJacobiansColumnsNames();
  111.             for (int j = 0; j < names.size(); ++j) {
  112.                 final double[] column = shortPeriodDerivativesJacobianColumns.get(names.get(j));
  113.                 for (int i = 0; i < STATE_DIMENSION; i++) {
  114.                     jacobian.addToEntry(i, j, column[i]);
  115.                 }
  116.             }

  117.         }

  118.         return jacobian;

  119.     }

  120.     /** Get the Jacobian matrix B1 (B1 = ∂εη/∂Y).
  121.      * <p>
  122.      * B1 represents the partial derivatives of the short period motion
  123.      * with respect to the mean equinoctial elements.
  124.      * </p>
  125.      * @return the B1 jacobian matrix
  126.      */
  127.     public RealMatrix getB1() {

  128.         // Initialize B1
  129.         final RealMatrix B1 = MatrixUtils.createRealMatrix(STATE_DIMENSION, STATE_DIMENSION);

  130.         // add the short period terms
  131.         for (int i = 0; i < STATE_DIMENSION; i++) {
  132.             for (int j = 0; j < STATE_DIMENSION; j++) {
  133.                 B1.addToEntry(i, j, shortPeriodDerivativesStm[i][j]);
  134.             }
  135.         }

  136.         // Return B1
  137.         return B1;

  138.     }

  139.     /** Get the Jacobian matrix B2 (B2 = ∂Y/∂Y₀).
  140.      * <p>
  141.      * B2 represents the partial derivatives of the mean equinoctial elements
  142.      * with respect to the initial ones.
  143.      * </p>
  144.      * @param state spacecraft state
  145.      * @return the B2 jacobian matrix
  146.      */
  147.     public RealMatrix getB2(final SpacecraftState state) {
  148.         return super.getStateTransitionMatrix(state);
  149.     }

  150.     /** Get the Jacobian matrix B3 (B3 = ∂Y/∂P).
  151.      * <p>
  152.      * B3 represents the partial derivatives of the mean equinoctial elements
  153.      * with respect to the estimated propagation parameters.
  154.      * </p>
  155.      * @param state spacecraft state
  156.      * @return the B3 jacobian matrix
  157.      */
  158.     public RealMatrix getB3(final SpacecraftState state) {
  159.         return super.getParametersJacobian(state);
  160.     }

  161.     /** Get the Jacobian matrix B4 (B4 = ∂εη/∂c).
  162.      * <p>
  163.      * B4 represents the partial derivatives of the short period motion
  164.      * with respect to the estimated propagation parameters.
  165.      * </p>
  166.      * @return the B4 jacobian matrix
  167.      */
  168.     public RealMatrix getB4() {

  169.         // Initialize B4
  170.         final List<String> names = getJacobiansColumnsNames();
  171.         final RealMatrix B4 = MatrixUtils.createRealMatrix(STATE_DIMENSION, names.size());

  172.         // add the short period terms
  173.         for (int j = 0; j < names.size(); ++j) {
  174.             final double[] column = shortPeriodDerivativesJacobianColumns.get(names.get(j));
  175.             for (int i = 0; i < STATE_DIMENSION; i++) {
  176.                 B4.addToEntry(i, j, column[i]);
  177.             }
  178.         }

  179.         // Return B4
  180.         return B4;

  181.     }

  182.     /** Freeze the names of the Jacobian columns.
  183.      * <p>
  184.      * This method is called when proagation starts, i.e. when configuration is completed
  185.      * </p>
  186.      */
  187.     public void freezeColumnsNames() {
  188.         columnsNames = getJacobiansColumnsNames();
  189.     }

  190.     /** {@inheritDoc} */
  191.     @Override
  192.     public List<String> getJacobiansColumnsNames() {
  193.         return columnsNames == null ? propagator.getJacobiansColumnsNames() : columnsNames;
  194.     }

  195.     /** Initialize the short periodic terms for the "field" elements.
  196.      * @param reference current mean spacecraft state
  197.      */
  198.     public void initializeFieldShortPeriodTerms(final SpacecraftState reference) {

  199.         // Converter
  200.         final DSSTGradientConverter converter = new DSSTGradientConverter(reference, propagator.getAttitudeProvider());

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

  203.             // Convert to Gradient
  204.             final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
  205.             final Gradient[] dsParameters = converter.getParametersAtStateDate(dsState, forceModel);
  206.             final FieldAuxiliaryElements<Gradient> auxiliaryElements = new FieldAuxiliaryElements<>(dsState.getOrbit(), I);

  207.             // Initialize the "Field" short periodic terms in OSCULATING mode
  208.             fieldShortPeriodTerms.addAll(forceModel.initializeShortPeriodTerms(auxiliaryElements, PropagationType.OSCULATING, dsParameters));

  209.         }

  210.     }

  211.     /** Update the short periodic terms for the "field" elements.
  212.      * @param reference current mean spacecraft state
  213.      */
  214.     @SuppressWarnings("unchecked")
  215.     public void updateFieldShortPeriodTerms(final SpacecraftState reference) {

  216.         // Converter
  217.         final DSSTGradientConverter converter = new DSSTGradientConverter(reference, propagator.getAttitudeProvider());

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

  220.             // Convert to Gradient
  221.             final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
  222.             final Gradient[] dsParameters = converter.getParameters(dsState, forceModel);

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

  225.         }

  226.     }

  227.     /** {@inheritDoc} */
  228.     @Override
  229.     public void setReferenceState(final SpacecraftState reference) {

  230.         // reset derivatives to zero
  231.         for (final double[] row : shortPeriodDerivativesStm) {
  232.             Arrays.fill(row, 0.0);
  233.         }

  234.         shortPeriodDerivativesJacobianColumns.clear();

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

  236.         // Compute Jacobian
  237.         for (final DSSTForceModel forceModel : propagator.getAllForceModels()) {

  238.             final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
  239.             final Gradient zero = dsState.getDate().getField().getZero();
  240.             final Gradient[] shortPeriod = new Gradient[6];
  241.             Arrays.fill(shortPeriod, zero);
  242.             for (final FieldShortPeriodTerms<Gradient> spt : fieldShortPeriodTerms) {
  243.                 final Gradient[] spVariation = spt.value(dsState.getOrbit());
  244.                 for (int i = 0; i < spVariation .length; i++) {
  245.                     shortPeriod[i] = shortPeriod[i].add(spVariation[i]);
  246.                 }
  247.             }

  248.             final double[] derivativesASP  = shortPeriod[0].getGradient();
  249.             final double[] derivativesExSP = shortPeriod[1].getGradient();
  250.             final double[] derivativesEySP = shortPeriod[2].getGradient();
  251.             final double[] derivativesHxSP = shortPeriod[3].getGradient();
  252.             final double[] derivativesHySP = shortPeriod[4].getGradient();
  253.             final double[] derivativesLSP  = shortPeriod[5].getGradient();

  254.             // update Jacobian with respect to state
  255.             addToRow(derivativesASP,  0);
  256.             addToRow(derivativesExSP, 1);
  257.             addToRow(derivativesEySP, 2);
  258.             addToRow(derivativesHxSP, 3);
  259.             addToRow(derivativesHySP, 4);
  260.             addToRow(derivativesLSP,  5);

  261.             int paramsIndex = converter.getFreeStateParameters();
  262.             for (ParameterDriver driver : forceModel.getParametersDrivers()) {
  263.                 if (driver.isSelected()) {

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

  266.                     for (Span<String> span = driverNameSpanMap.getFirstSpan(); span != null; span = span.next()) {
  267.                         // get the partials derivatives for this driver
  268.                         DoubleArrayDictionary.Entry entry = shortPeriodDerivativesJacobianColumns.getEntry(span.getData());
  269.                         if (entry == null) {
  270.                             // create an entry filled with zeroes
  271.                             shortPeriodDerivativesJacobianColumns.put(span.getData(), new double[STATE_DIMENSION]);
  272.                             entry = shortPeriodDerivativesJacobianColumns.getEntry(span.getData());
  273.                         }

  274.                         // add the contribution of the current force model
  275.                         entry.increment(new double[] {
  276.                             derivativesASP[paramsIndex], derivativesExSP[paramsIndex], derivativesEySP[paramsIndex],
  277.                             derivativesHxSP[paramsIndex], derivativesHySP[paramsIndex], derivativesLSP[paramsIndex]
  278.                         });
  279.                         ++paramsIndex;
  280.                     }
  281.                 }
  282.             }
  283.         }

  284.     }

  285.     /** Fill State Transition Matrix 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.      */
  289.     private void addToRow(final double[] derivatives, final int index) {
  290.         for (int i = 0; i < 6; i++) {
  291.             shortPeriodDerivativesStm[index][i] += derivatives[i];
  292.         }
  293.     }

  294.     /** {@inheritDoc} */
  295.     @Override
  296.     public OrbitType getOrbitType() {
  297.         return propagator.getOrbitType();
  298.     }

  299.     /** {@inheritDoc} */
  300.     @Override
  301.     public PositionAngleType getPositionAngleType() {
  302.         return propagator.getPositionAngleType();
  303.     }

  304. }