DSSTHarvester.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.ArrayList;
  20. import java.util.Arrays;
  21. import java.util.List;

  23. import org.hipparchus.analysis.differentiation.Gradient;
  24. import org.hipparchus.linear.MatrixUtils;
  25. import org.hipparchus.linear.RealMatrix;
  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;

  36. /** Harvester between two-dimensional Jacobian matrices and one-dimensional {@link
  37.  * SpacecraftState#getAdditionalState(String) additional state arrays}.
  38.  * @author Luc Maisonobe
  39.  * @author Bryan Cazabonne
  40.  * @since 11.1
  41.  */
  42. 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;

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

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

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

  68.     /** Columns names for parameters. */
  69.     private List<String> columnsNames;

  71.     /** Field short periodic terms. */
  72.     private List<FieldShortPeriodTerms<Gradient>> fieldShortPeriodTerms;

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

  97.     /** {@inheritDoc} */
  98.     @Override
  99.     public RealMatrix getStateTransitionMatrix(final SpacecraftState state) {

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

  103.         if (propagator.getPropagationType() == PropagationType.OSCULATING) {
  104.             // add the short period terms
  105.             for (int i = 0; i < STATE_DIMENSION; i++) {
  106.                 for (int j = 0; j < STATE_DIMENSION; j++) {
  107.                     stm.addToEntry(i, j, shortPeriodDerivativesStm[i][j]);
  108.                 }
  109.             }
  110.         }

  112.         return stm;

  114.     }

  116.     /** {@inheritDoc} */
  117.     @Override
  118.     public RealMatrix getParametersJacobian(final SpacecraftState state) {

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

  123.             // add the short period terms
  124.             final List<String> names = getJacobiansColumnsNames();
  125.             for (int j = 0; j < names.size(); ++j) {
  126.                 final double[] column = shortPeriodDerivativesJacobianColumns.get(names.get(j));
  127.                 for (int i = 0; i < STATE_DIMENSION; i++) {
  128.                     jacobian.addToEntry(i, j, column[i]);
  129.                 }
  130.             }

  132.         }

  134.         return jacobian;

  136.     }

  138.     /** Get the Jacobian matrix B1 (B1 = ∂εη/∂Y).
  139.      * <p>
  140.      * B1 represents the partial derivatives of the short period motion
  141.      * with respect to the mean equinoctial elements.
  142.      * </p>
  143.      * @return the B1 jacobian matrix
  144.      */
  145.     public RealMatrix getB1() {

  147.         // Initialize B1
  148.         final RealMatrix B1 = MatrixUtils.createRealMatrix(STATE_DIMENSION, STATE_DIMENSION);

  150.         // add the short period terms
  151.         for (int i = 0; i < STATE_DIMENSION; i++) {
  152.             for (int j = 0; j < STATE_DIMENSION; j++) {
  153.                 B1.addToEntry(i, j, shortPeriodDerivativesStm[i][j]);
  154.             }
  155.         }

  157.         // Return B1
  158.         return B1;

  160.     }

  162.     /** Get the Jacobian matrix B2 (B2 = ∂Y/∂Y₀).
  163.      * <p>
  164.      * B2 represents the partial derivatives of the mean equinoctial elements
  165.      * with respect to the initial ones.
  166.      * </p>
  167.      * @param state spacecraft state
  168.      * @return the B2 jacobian matrix
  169.      */
  170.     public RealMatrix getB2(final SpacecraftState state) {
  171.         return super.getStateTransitionMatrix(state);
  172.     }

  174.     /** Get the Jacobian matrix B3 (B3 = ∂Y/∂P).
  175.      * <p>
  176.      * B3 represents the partial derivatives of the mean equinoctial elements
  177.      * with respect to the estimated propagation parameters.
  178.      * </p>
  179.      * @param state spacecraft state
  180.      * @return the B3 jacobian matrix
  181.      */
  182.     public RealMatrix getB3(final SpacecraftState state) {
  183.         return super.getParametersJacobian(state);
  184.     }

  186.     /** Get the Jacobian matrix B4 (B4 = ∂εη/∂c).
  187.      * <p>
  188.      * B4 represents the partial derivatives of the short period motion
  189.      * with respect to the estimated propagation parameters.
  190.      * </p>
  191.      * @return the B4 jacobian matrix
  192.      */
  193.     public RealMatrix getB4() {

  195.         // Initialize B4
  196.         final List<String> names = getJacobiansColumnsNames();
  197.         final RealMatrix B4 = MatrixUtils.createRealMatrix(STATE_DIMENSION, names.size());

  199.         // add the short period terms
  200.         for (int j = 0; j < names.size(); ++j) {
  201.             final double[] column = shortPeriodDerivativesJacobianColumns.get(names.get(j));
  202.             for (int i = 0; i < STATE_DIMENSION; i++) {
  203.                 B4.addToEntry(i, j, column[i]);
  204.             }
  205.         }

  207.         // Return B4
  208.         return B4;

  210.     }

  212.     /** Freeze the names of the Jacobian columns.
  213.      * <p>
  214.      * This method is called when proagation starts, i.e. when configuration is completed
  215.      * </p>
  216.      */
  217.     public void freezeColumnsNames() {
  218.         columnsNames = getJacobiansColumnsNames();
  219.     }

  221.     /** {@inheritDoc} */
  222.     @Override
  223.     public List<String> getJacobiansColumnsNames() {
  224.         return columnsNames == null ? propagator.getJacobiansColumnsNames() : columnsNames;
  225.     }

  227.     /** Initialize the short periodic terms for the "field" elements.
  228.      * @param reference current mean spacecraft state
  229.      */
  230.     public void initializeFieldShortPeriodTerms(final SpacecraftState reference) {

  232.         // Converter
  233.         final DSSTGradientConverter converter = new DSSTGradientConverter(reference, propagator.getAttitudeProvider());

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

  238.             // Convert to Gradient
  239.             final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
  240.             final Gradient[] dsParameters = converter.getParameters(dsState, forceModel);
  241.             final FieldAuxiliaryElements<Gradient> auxiliaryElements = new FieldAuxiliaryElements<>(dsState.getOrbit(), I);

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

  246.         }

  248.     }

  250.     /** Update the short periodic terms for the "field" elements.
  251.      * @param reference current mean spacecraft state
  252.      */
  253.     @SuppressWarnings("unchecked")
  254.     public void updateFieldShortPeriodTerms(final SpacecraftState reference) {

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

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

  262.             // Convert to Gradient
  263.             final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
  264.             final Gradient[] dsParameters = converter.getParameters(dsState, forceModel);

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

  269.         }

  271.     }

  273.     /** {@inheritDoc} */
  274.     @Override
  275.     public void setReferenceState(final SpacecraftState reference) {

  277.         // reset derivatives to zero
  278.         for (final double[] row : shortPeriodDerivativesStm) {
  279.             Arrays.fill(row, 0.0);
  280.         }

  282.         shortPeriodDerivativesJacobianColumns.clear();

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

  286.         // Compute Jacobian
  287.         for (final DSSTForceModel forceModel : propagator.getAllForceModels()) {

  289.             final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
  290.             final Gradient zero = dsState.getDate().getField().getZero();
  291.             final Gradient[] shortPeriod = new Gradient[6];
  292.             Arrays.fill(shortPeriod, zero);
  293.             for (final FieldShortPeriodTerms<Gradient> spt : fieldShortPeriodTerms) {
  294.                 final Gradient[] spVariation = spt.value(dsState.getOrbit());
  295.                 for (int i = 0; i < spVariation .length; i++) {
  296.                     shortPeriod[i] = shortPeriod[i].add(spVariation[i]);
  297.                 }
  298.             }

  300.             final double[] derivativesASP  = shortPeriod[0].getGradient();
  301.             final double[] derivativesExSP = shortPeriod[1].getGradient();
  302.             final double[] derivativesEySP = shortPeriod[2].getGradient();
  303.             final double[] derivativesHxSP = shortPeriod[3].getGradient();
  304.             final double[] derivativesHySP = shortPeriod[4].getGradient();
  305.             final double[] derivativesLSP  = shortPeriod[5].getGradient();

  307.             // update Jacobian with respect to state
  308.             addToRow(derivativesASP,  0);
  309.             addToRow(derivativesExSP, 1);
  310.             addToRow(derivativesEySP, 2);
  311.             addToRow(derivativesHxSP, 3);
  312.             addToRow(derivativesHySP, 4);
  313.             addToRow(derivativesLSP,  5);

  315.             int paramsIndex = converter.getFreeStateParameters();
  316.             for (ParameterDriver driver : forceModel.getParametersDrivers()) {
  317.                 if (driver.isSelected()) {

  319.                     // get the partials derivatives for this driver
  320.                     DoubleArrayDictionary.Entry entry = shortPeriodDerivativesJacobianColumns.getEntry(driver.getName());
  321.                     if (entry == null) {
  322.                         // create an entry filled with zeroes
  323.                         shortPeriodDerivativesJacobianColumns.put(driver.getName(), new double[STATE_DIMENSION]);
  324.                         entry = shortPeriodDerivativesJacobianColumns.getEntry(driver.getName());
  325.                     }

  327.                     // add the contribution of the current force model
  328.                     entry.increment(new double[] {
  329.                         derivativesASP[paramsIndex], derivativesExSP[paramsIndex], derivativesEySP[paramsIndex],
  330.                         derivativesHxSP[paramsIndex], derivativesHySP[paramsIndex], derivativesLSP[paramsIndex]
  331.                     });
  332.                     ++paramsIndex;

  334.                 }
  335.             }
  336.         }

  338.     }

  340.     /** Fill State Transition Matrix rows.
  341.      * @param derivatives derivatives of a component
  342.      * @param index component index (0 for a, 1 for ex, 2 for ey, 3 for hx, 4 for hy, 5 for l)
  343.      */
  344.     private void addToRow(final double[] derivatives, final int index) {
  345.         for (int i = 0; i < 6; i++) {
  346.             shortPeriodDerivativesStm[index][i] += derivatives[i];
  347.         }
  348.     }

  350. }
Created with JaCoCo 0.8.7.202105040129