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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  149.             }

  151.         }

  153.     }

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  221.             }

  223.         }

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

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

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

  268.         }

  270.     }

  272. }
Created with JaCoCo 0.8.8.202204050719