AbstractAnalyticalMatricesHarvester.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.analytical;

  19. import java.util.Arrays;
  20. import java.util.List;

  22. import org.hipparchus.analysis.differentiation.Gradient;
  23. import org.hipparchus.linear.MatrixUtils;
  24. import org.hipparchus.linear.RealMatrix;
  25. import org.orekit.orbits.FieldOrbit;
  26. import org.orekit.orbits.OrbitType;
  27. import org.orekit.orbits.PositionAngleType;
  28. import org.orekit.propagation.AbstractMatricesHarvester;
  29. import org.orekit.propagation.AdditionalDataProvider;
  30. import org.orekit.propagation.FieldSpacecraftState;
  31. import org.orekit.propagation.SpacecraftState;
  32. import org.orekit.time.AbsoluteDate;
  33. import org.orekit.time.FieldAbsoluteDate;
  34. import org.orekit.utils.DoubleArrayDictionary;
  35. import org.orekit.utils.FieldPVCoordinates;
  36. import org.orekit.utils.ParameterDriver;
  37. import org.orekit.utils.TimeSpanMap;
  38. import org.orekit.utils.TimeSpanMap.Span;

  40. /**
  41.  * Base class harvester between two-dimensional Jacobian
  42.  * matrices and analytical orbit propagator.
  43.  * @author Thomas Paulet
  44.  * @author Bryan Cazabonne
  45.  * @since 11.1
  46.  */
  47. public abstract class AbstractAnalyticalMatricesHarvester extends AbstractMatricesHarvester
  48.         implements AdditionalDataProvider<double[]> {

  50.     /** Columns names for parameters. */
  51.     private List<String> columnsNames;

  53.     /** Epoch of the last computed state transition matrix. */
  54.     private AbsoluteDate epoch;

  56.     /** Analytical derivatives that apply to State Transition Matrix. */
  57.     private final double[][] analyticalDerivativesStm;

  59.     /** Analytical derivatives that apply to Jacobians columns. */
  60.     private final DoubleArrayDictionary analyticalDerivativesJacobianColumns;

  62.     /** Propagator bound to this harvester. */
  63.     private final AbstractAnalyticalPropagator propagator;

  65.     /** Simple constructor.
  66.      * <p>
  67.      * The arguments for initial matrices <em>must</em> be compatible with the
  68.      * {@link org.orekit.orbits.OrbitType orbit type}
  69.      * and {@link PositionAngleType position angle} that will be used by propagator
  70.      * </p>
  71.      * @param propagator propagator bound to this harvester
  72.      * @param stmName State Transition Matrix state name
  73.      * @param initialStm initial State Transition Matrix ∂Y/∂Y₀,
  74.      * if null (which is the most frequent case), assumed to be 6x6 identity
  75.      * @param initialJacobianColumns initial columns of the Jacobians matrix with respect to parameters,
  76.      * if null or if some selected parameters are missing from the dictionary, the corresponding
  77.      * initial column is assumed to be 0
  78.      */
  79.     protected AbstractAnalyticalMatricesHarvester(final AbstractAnalyticalPropagator propagator, final String stmName,
  80.                                                   final RealMatrix initialStm, final DoubleArrayDictionary initialJacobianColumns) {
  81.         super(stmName, initialStm, initialJacobianColumns);
  82.         this.propagator                           = propagator;
  83.         this.epoch                                = propagator.getInitialState().getDate();
  84.         this.columnsNames                         = null;
  85.         this.analyticalDerivativesStm             = getInitialStateTransitionMatrix().getData();
  86.         this.analyticalDerivativesJacobianColumns = new DoubleArrayDictionary();
  87.     }

  89.     /** {@inheritDoc} */
  90.     @Override
  91.     public List<String> getJacobiansColumnsNames() {
  92.         return columnsNames == null ? propagator.getJacobiansColumnsNames() : columnsNames;
  93.     }

  95.     /** {@inheritDoc} */
  96.     @Override
  97.     public void freezeColumnsNames() {
  98.         columnsNames = getJacobiansColumnsNames();
  99.     }

  101.     /** {@inheritDoc} */
  102.     @Override
  103.     public String getName() {
  104.         return getStmName();
  105.     }

  107.     /** {@inheritDoc} */
  108.     @Override
  109.     public double[] getAdditionalData(final SpacecraftState state) {
  110.         // Update the partial derivatives if needed
  111.         updateDerivativesIfNeeded(state);
  112.         // Return the state transition matrix in an array
  113.         return toArray(analyticalDerivativesStm);
  114.     }

  116.     /** {@inheritDoc} */
  117.     @Override
  118.     public RealMatrix getStateTransitionMatrix(final SpacecraftState state) {
  119.         // Check if additional state is defined
  120.         if (!state.hasAdditionalData(getName())) {
  121.             return null;
  122.         }
  123.         // Return the state transition matrix
  124.         return toSquareMatrix(state.getAdditionalState(getName()));
  125.     }

  127.     /** {@inheritDoc} */
  128.     @Override
  129.     public RealMatrix getParametersJacobian(final SpacecraftState state) {
  130.         // Update the partial derivatives if needed
  131.         updateDerivativesIfNeeded(state);

  133.         // Estimated parameters
  134.         final List<String> names = getJacobiansColumnsNames();
  135.         if (names == null || names.isEmpty()) {
  136.             return null;
  137.         }

  139.         // Initialize Jacobian
  140.         final RealMatrix dYdP = MatrixUtils.createRealMatrix(getStateDimension(), names.size());

  142.         // Add the derivatives
  143.         for (int j = 0; j < names.size(); ++j) {
  144.             final double[] column = analyticalDerivativesJacobianColumns.get(names.get(j));
  145.             if (column != null) {
  146.                 for (int i = 0; i < getStateDimension(); i++) {
  147.                     dYdP.addToEntry(i, j, column[i]);
  148.                 }
  149.             }
  150.         }

  152.         // Return
  153.         return dYdP;
  154.     }

  156.     /** {@inheritDoc} */
  157.     @Override
  158.     public void setReferenceState(final SpacecraftState reference) {

  160.         // reset derivatives to zero
  161.         for (final double[] row : analyticalDerivativesStm) {
  162.             Arrays.fill(row, 0.0);
  163.         }
  164.         analyticalDerivativesJacobianColumns.clear();

  166.         final AbstractAnalyticalGradientConverter converter           = getGradientConverter();
  167.         final FieldAbstractAnalyticalPropagator<Gradient> gPropagator = converter.getPropagator();

  169.         // Compute Jacobian
  170.         final AbsoluteDate                   target     = reference.getDate();
  171.         final FieldAbsoluteDate<Gradient>    start      = gPropagator.getInitialState().getDate();
  172.         final double                         dt         = target.durationFrom(start.toAbsoluteDate());
  173.         final FieldSpacecraftState<Gradient> state      = gPropagator.getInitialState();
  174.         final Gradient[]                     parameters = converter.getParameters(state, converter);
  175.         final FieldOrbit<Gradient>           gOrbit     = gPropagator.propagateOrbit(start.shiftedBy(dt), parameters);
  176.         final FieldPVCoordinates<Gradient>   gPv        = gOrbit.getPVCoordinates();

  178.         final double[] derivativesX  = gPv.getPosition().getX().getGradient();
  179.         final double[] derivativesY  = gPv.getPosition().getY().getGradient();
  180.         final double[] derivativesZ  = gPv.getPosition().getZ().getGradient();
  181.         final double[] derivativesVx = gPv.getVelocity().getX().getGradient();
  182.         final double[] derivativesVy = gPv.getVelocity().getY().getGradient();
  183.         final double[] derivativesVz = gPv.getVelocity().getZ().getGradient();

  185.         // Update Jacobian with respect to state
  186.         addToRow(derivativesX,  0);
  187.         addToRow(derivativesY,  1);
  188.         addToRow(derivativesZ,  2);
  189.         addToRow(derivativesVx, 3);
  190.         addToRow(derivativesVy, 4);
  191.         addToRow(derivativesVz, 5);

  193.         // Partial derivatives of the state with respect to propagation parameters
  194.         int paramsIndex = converter.getFreeStateParameters();
  195.         for (ParameterDriver driver : converter.getParametersDrivers()) {
  196.             if (driver.isSelected()) {

  198.                 final TimeSpanMap<String> driverNameSpanMap = driver.getNamesSpanMap();
  199.                 // for each span (for each estimated value) corresponding name is added
  200.                 for (Span<String> span = driverNameSpanMap.getFirstSpan(); span != null; span = span.next()) {
  201.                     // get the partials derivatives for this driver
  202.                     DoubleArrayDictionary.Entry entry = analyticalDerivativesJacobianColumns.getEntry(span.getData());
  203.                     if (entry == null) {
  204.                         // create an entry filled with zeroes
  205.                         analyticalDerivativesJacobianColumns.put(span.getData(), new double[getStateDimension()]);
  206.                         entry = analyticalDerivativesJacobianColumns.getEntry(span.getData());
  207.                     }

  209.                     // add the contribution of the current force model
  210.                     entry.increment(new double[] {
  211.                         derivativesX[paramsIndex], derivativesY[paramsIndex], derivativesZ[paramsIndex],
  212.                         derivativesVx[paramsIndex], derivativesVy[paramsIndex], derivativesVz[paramsIndex]
  213.                     });
  214.                     ++paramsIndex;
  215.                 }
  216.             }
  217.         }

  219.         // Update the epoch of the last computed partial derivatives
  220.         epoch = target;

  222.     }

  224.     /** Update the partial derivatives (if needed).
  225.      * @param state current spacecraft state
  226.      */
  227.     private void updateDerivativesIfNeeded(final SpacecraftState state) {
  228.         if (!state.getDate().isEqualTo(epoch)) {
  229.             setReferenceState(state);
  230.         }
  231.     }

  233.     /** Fill State Transition Matrix rows.
  234.      * @param derivatives derivatives of a component
  235.      * @param index component index
  236.      */
  237.     private void addToRow(final double[] derivatives, final int index) {
  238.         for (int i = 0; i < 6; i++) {
  239.             analyticalDerivativesStm[index][i] += derivatives[i];
  240.         }
  241.     }

  243.     /** {@inheritDoc} */
  244.     @Override
  245.     public OrbitType getOrbitType() {
  246.         // Set to CARTESIAN because analytical gradient converters uses Cartesian representation
  247.         return OrbitType.CARTESIAN;
  248.     }

  250.     /** {@inheritDoc} */
  251.     @Override
  252.     public PositionAngleType getPositionAngleType() {
  253.         // Irrelevant: set a default value
  254.         return PositionAngleType.MEAN;
  255.     }

  257.     /**
  258.      * Get the gradient converter related to the analytical orbit propagator.
  259.      * @return the gradient converter
  260.      */
  261.     public abstract AbstractAnalyticalGradientConverter getGradientConverter();

  263. }
Created with JaCoCo 0.8.12.202403310830