JacobianPropagatorConverter.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.conversion;

  19. import java.util.List;

  21. import org.hipparchus.analysis.MultivariateVectorFunction;
  22. import org.hipparchus.linear.ArrayRealVector;
  23. import org.hipparchus.linear.MatrixUtils;
  24. import org.hipparchus.linear.RealMatrix;
  25. import org.hipparchus.linear.RealVector;
  26. import org.hipparchus.optim.nonlinear.vector.leastsquares.MultivariateJacobianFunction;
  27. import org.hipparchus.util.Pair;
  28. import org.orekit.errors.OrekitException;
  29. import org.orekit.errors.OrekitMessages;
  30. import org.orekit.orbits.OrbitType;
  31. import org.orekit.propagation.MatricesHarvester;
  32. import org.orekit.propagation.SpacecraftState;
  33. import org.orekit.propagation.numerical.NumericalPropagator;
  34. import org.orekit.propagation.sampling.OrekitStepHandler;
  35. import org.orekit.propagation.sampling.OrekitStepInterpolator;
  36. import org.orekit.time.AbsoluteDate;
  37. import org.orekit.utils.PVCoordinates;
  38. import org.orekit.utils.ParameterDriver;
  39. import org.orekit.utils.ParameterDriversList;
  40. import org.orekit.utils.TimeSpanMap.Span;

  42. /** Propagator converter using the real Jacobian.
  43.  * @author Pascal Parraud
  44.  * @since 6.0
  45.  */
  46. public class JacobianPropagatorConverter extends AbstractPropagatorConverter {

  48.     /** Numerical propagator builder. */
  49.     private final NumericalPropagatorBuilder builder;

  51.     /** Simple constructor.
  52.      * @param builder builder for adapted propagator, it <em>must</em>
  53.      * be configured to generate {@link OrbitType#CARTESIAN} states
  54.      * @param threshold absolute threshold for optimization algorithm
  55.      * @param maxIterations maximum number of iterations for fitting
  56.      */
  57.     public JacobianPropagatorConverter(final NumericalPropagatorBuilder builder,
  58.                                        final double threshold,
  59.                                        final int maxIterations) {
  60.         super(builder, threshold, maxIterations);
  61.         if (builder.getOrbitType() != OrbitType.CARTESIAN) {
  62.             throw new OrekitException(OrekitMessages.ORBIT_TYPE_NOT_ALLOWED,
  63.                                       builder.getOrbitType(), OrbitType.CARTESIAN);
  64.         }
  65.         this.builder = builder;
  66.     }

  68.     /** {@inheritDoc} */
  69.     protected MultivariateVectorFunction getObjectiveFunction() {
  70.         return point -> {
  71.             final NumericalPropagator propagator  = builder.buildPropagator(point);
  72.             final ValuesHandler handler = new ValuesHandler();
  73.             propagator.getMultiplexer().add(handler);
  74.             final List<SpacecraftState> sample = getSample();
  75.             propagator.propagate(sample.get(sample.size() - 1).getDate().shiftedBy(10.0));
  76.             return handler.value;
  77.         };
  78.     }

  80.     /** {@inheritDoc} */
  81.     protected MultivariateJacobianFunction getModel() {
  82.         return point -> {
  83.             final NumericalPropagator propagator  = builder.buildPropagator(point.toArray());
  84.             final JacobianHandler handler = new JacobianHandler(propagator, point.getDimension());
  85.             propagator.getMultiplexer().add(handler);
  86.             final List<SpacecraftState> sample = getSample();
  87.             propagator.propagate(sample.get(sample.size() - 1).getDate().shiftedBy(10.0));
  88.             return new Pair<>(handler.value, handler.jacobian);
  89.         };
  90.     }

  92.     /** Handler for picking up values at sample dates.
  93.      * <p>
  94.      * This class is heavily based on org.orekit.estimation.leastsquares.MeasurementHandler.
  95.      * </p>
  96.      * @since 11.1
  97.      */
  98.     private class ValuesHandler implements OrekitStepHandler {

  100.         /** Values vector. */
  101.         private final double[] value;

  103.         /** Number of the next measurement. */
  104.         private int number;

  106.         /** Index of the next component in the model. */
  107.         private int index;

  109.         /** Simple constructor.
  110.          */
  111.         ValuesHandler() {
  112.             this.value = new double[getTargetSize()];
  113.         }

  115.         /** {@inheritDoc} */
  116.         @Override
  117.         public void init(final SpacecraftState initialState, final AbsoluteDate target) {
  118.             number = 0;
  119.             index  = 0;
  120.         }

  122.         /** {@inheritDoc} */
  123.         @Override
  124.         public void handleStep(final OrekitStepInterpolator interpolator) {

  126.             while (number < getSample().size()) {

  128.                 // Consider the next sample to handle
  129.                 final SpacecraftState next = getSample().get(number);

  131.                 // Current state date
  132.                 final AbsoluteDate currentDate = interpolator.getCurrentState().getDate();
  133.                 if (next.getDate().compareTo(currentDate) > 0) {
  134.                     return;
  135.                 }

  137.                 final PVCoordinates pv = interpolator.getInterpolatedState(next.getDate()).getPVCoordinates(getFrame());
  138.                 value[index++] = pv.getPosition().getX();
  139.                 value[index++] = pv.getPosition().getY();
  140.                 value[index++] = pv.getPosition().getZ();
  141.                 if (!isOnlyPosition()) {
  142.                     value[index++] = pv.getVelocity().getX();
  143.                     value[index++] = pv.getVelocity().getY();
  144.                     value[index++] = pv.getVelocity().getZ();
  145.                 }

  147.                 // prepare handling of next measurement
  148.                 ++number;

  150.             }

  152.         }

  154.     }

  156.     /** Handler for picking up Jacobians at sample dates.
  157.      * <p>
  158.      * This class is heavily based on org.orekit.estimation.leastsquares.MeasurementHandler.
  159.      * </p>
  160.      * @since 11.1
  161.      */
  162.     private class JacobianHandler implements OrekitStepHandler {

  164.         /** Values vector. */
  165.         private final RealVector value;

  167.         /** Jacobian matrix. */
  168.         private final RealMatrix jacobian;

  170.         /** State size (3 or 6). */
  171.         private final int stateSize;

  173.         /** Matrices harvester. */
  174.         private final MatricesHarvester harvester;

  176.         /** Number of the next measurement. */
  177.         private int number;

  179.         /** Index of the next Jacobian component in the model. */
  180.         private int index;

  182.         /** Simple constructor.
  183.          * @param propagator propagator
  184.          * @param columns number of columns of the Jacobian matrix
  185.          */
  186.         JacobianHandler(final NumericalPropagator propagator, final int columns) {
  187.             this.value     = new ArrayRealVector(getTargetSize());
  188.             this.jacobian  = MatrixUtils.createRealMatrix(getTargetSize(), columns);
  189.             this.stateSize = isOnlyPosition() ? 3 : 6;
  190.             this.harvester = propagator.setupMatricesComputation("converter-partials", null, null);
  191.         }

  193.         /** {@inheritDoc} */
  194.         @Override
  195.         public void init(final SpacecraftState initialState, final AbsoluteDate target) {
  196.             number = 0;
  197.             index  = 0;
  198.         }

  200.         /** {@inheritDoc} */
  201.         @Override
  202.         public void handleStep(final OrekitStepInterpolator interpolator) {

  204.             while (number < getSample().size()) {

  206.                 // Consider the next sample to handle
  207.                 final SpacecraftState next = getSample().get(number);

  209.                 // Current state date
  210.                 final AbsoluteDate currentDate = interpolator.getCurrentState().getDate();
  211.                 if (next.getDate().compareTo(currentDate) > 0) {
  212.                     return;
  213.                 }

  215.                 fillRows(index, interpolator.getInterpolatedState(next.getDate()),
  216.                          builder.getOrbitalParametersDrivers());

  218.                 // prepare handling of next measurement
  219.                 ++number;
  220.                 index += stateSize;

  222.             }

  224.         }

  226.         /** Fill up a few Jacobian rows (either 6 or 3 depending on velocities used or not).
  227.          * @param row first row index
  228.          * @param state spacecraft state
  229.          * @param orbitalParameters drivers for the orbital parameters
  230.          */
  231.         private void fillRows(final int row,
  232.                               final SpacecraftState state,
  233.                               final ParameterDriversList orbitalParameters) {

  235.             // value part
  236.             final PVCoordinates pv = state.getPVCoordinates(getFrame());
  237.             value.setEntry(row,     pv.getPosition().getX());
  238.             value.setEntry(row + 1, pv.getPosition().getY());
  239.             value.setEntry(row + 2, pv.getPosition().getZ());
  240.             if (!isOnlyPosition()) {
  241.                 value.setEntry(row + 3, pv.getVelocity().getX());
  242.                 value.setEntry(row + 4, pv.getVelocity().getY());
  243.                 value.setEntry(row + 5, pv.getVelocity().getZ());
  244.             }

  246.             // Jacobian part
  247.             final RealMatrix dYdY0 = harvester.getStateTransitionMatrix(state);
  248.             final RealMatrix dYdP  = harvester.getParametersJacobian(state);
  249.             for (int k = 0; k < stateSize; k++) {
  250.                 int column = 0;
  251.                 for (int j = 0; j < orbitalParameters.getNbParams(); ++j) {
  252.                     final ParameterDriver driver = orbitalParameters.getDrivers().get(j);
  253.                     if (driver.isSelected()) {
  254.                         jacobian.setEntry(row + k, column++, dYdY0.getEntry(k, j) * driver.getScale());
  255.                     }
  256.                 }
  257.                 if (dYdP != null) {
  258.                     for (int j = 0; j < dYdP.getColumnDimension(); ++j) {
  259.                         final String name = harvester.getJacobiansColumnsNames().get(j);
  260.                         for (final ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {

  262.                             for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
  263.                                 if (name.equals(span.getData())) {
  264.                                     jacobian.setEntry(row + k, column++, dYdP.getEntry(k, j) * driver.getScale());
  265.                                 }
  266.                             }
  267.                         }
  268.                     }
  269.                 }
  270.             }
  271.         }

  273.     }

  275. }
Created with JaCoCo 0.8.12.202403310830