JacobianPropagatorConverter.java

  1. /* Copyright 2002-2020 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.ode.events.Action;
  27. import org.hipparchus.optim.nonlinear.vector.leastsquares.MultivariateJacobianFunction;
  28. import org.hipparchus.util.Pair;
  29. import org.orekit.errors.OrekitException;
  30. import org.orekit.errors.OrekitMessages;
  31. import org.orekit.orbits.OrbitType;
  32. import org.orekit.propagation.SpacecraftState;
  33. import org.orekit.propagation.events.DateDetector;
  34. import org.orekit.propagation.events.handlers.EventHandler;
  35. import org.orekit.propagation.numerical.JacobiansMapper;
  36. import org.orekit.propagation.numerical.NumericalPropagator;
  37. import org.orekit.propagation.numerical.PartialDerivativesEquations;
  38. import org.orekit.utils.PVCoordinates;
  39. import org.orekit.utils.ParameterDriver;
  40. import org.orekit.utils.ParameterDriversList;

  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 new MultivariateVectorFunction() {

  72.             /** {@inheritDoc} */
  73.             public double[] value(final double[] arg)
  74.                 throws IllegalArgumentException, OrekitException {

  76.                 final double[] value = new double[getTargetSize()];

  78.                 final NumericalPropagator prop = builder.buildPropagator(arg);

  80.                 final int stateSize = isOnlyPosition() ? 3 : 6;
  81.                 final List<SpacecraftState> sample = getSample();
  82.                 for (int i = 0; i < sample.size(); ++i) {
  83.                     final int row = i * stateSize;
  84.                     if (prop.getInitialState().getDate().equals(sample.get(i).getDate())) {
  85.                         // use initial state
  86.                         fillRows(value, row, prop.getInitialState());
  87.                     } else {
  88.                         // use a date detector to pick up states
  89.                         prop.addEventDetector(new DateDetector(sample.get(i).getDate()).withHandler(new EventHandler<DateDetector>() {
  90.                             /** {@inheritDoc} */
  91.                             @Override
  92.                             public Action eventOccurred(final SpacecraftState state, final DateDetector detector,
  93.                                                         final boolean increasing) {
  94.                                 fillRows(value, row, state);
  95.                                 return row + stateSize >= getTargetSize() ? Action.STOP : Action.CONTINUE;
  96.                             }
  97.                         }));
  98.                     }
  99.                 }

  101.                 prop.propagate(sample.get(sample.size() - 1).getDate().shiftedBy(10.0));

  103.                 return value;
  104.             }
  105.         };
  106.     }

  108.     /** {@inheritDoc} */
  109.     protected MultivariateJacobianFunction getModel() {
  110.         return new MultivariateJacobianFunction() {

  112.             /** {@inheritDoc} */
  113.             public Pair<RealVector, RealMatrix> value(final RealVector point)
  114.                 throws IllegalArgumentException, OrekitException {

  116.                 final RealVector value    = new ArrayRealVector(getTargetSize());
  117.                 final RealMatrix jacobian = MatrixUtils.createRealMatrix(getTargetSize(), point.getDimension());

  119.                 final NumericalPropagator prop  = builder.buildPropagator(point.toArray());
  120.                 final int stateSize = isOnlyPosition() ? 3 : 6;
  121.                 final ParameterDriversList orbitalParameters = builder.getOrbitalParametersDrivers();
  122.                 final PartialDerivativesEquations pde = new PartialDerivativesEquations("pde", prop);
  123.                 final ParameterDriversList propagationParameters = pde.getSelectedParameters();
  124.                 prop.setInitialState(pde.setInitialJacobians(prop.getInitialState()));
  125.                 final JacobiansMapper mapper  = pde.getMapper();

  127.                 final List<SpacecraftState> sample = getSample();
  128.                 for (int i = 0; i < sample.size(); ++i) {
  129.                     final int row = i * stateSize;
  130.                     if (prop.getInitialState().getDate().equals(sample.get(i).getDate())) {
  131.                         // use initial state and Jacobians
  132.                         fillRows(value, jacobian, row, prop.getInitialState(), stateSize,
  133.                                  orbitalParameters, propagationParameters, mapper);
  134.                     } else {
  135.                         // use a date detector to pick up state and Jacobians
  136.                         prop.addEventDetector(new DateDetector(sample.get(i).getDate()).withHandler(new EventHandler<DateDetector>() {
  137.                             /** {@inheritDoc} */
  138.                             @Override
  139.                             public Action eventOccurred(final SpacecraftState state, final DateDetector detector,
  140.                                                         final boolean increasing) {
  141.                                 fillRows(value, jacobian, row, state, stateSize,
  142.                                          orbitalParameters, propagationParameters, mapper);
  143.                                 return row + stateSize >= getTargetSize() ? Action.STOP : Action.CONTINUE;
  144.                             }
  145.                         }));
  146.                     }
  147.                 }

  149.                 prop.propagate(sample.get(sample.size() - 1).getDate().shiftedBy(10.0));

  151.                 return new Pair<RealVector, RealMatrix>(value, jacobian);
  152.             }
  153.         };
  154.     }

  156.     /** Fill up a few value rows (either 6 or 3 depending on velocities used or not).
  157.      * @param value values array
  158.      * @param row first row index
  159.      * @param state spacecraft state
  160.      */
  161.     private void fillRows(final double[] value, final int row, final SpacecraftState state) {
  162.         final PVCoordinates pv = state.getPVCoordinates(getFrame());
  163.         value[row    ] = pv.getPosition().getX();
  164.         value[row + 1] = pv.getPosition().getY();
  165.         value[row + 2] = pv.getPosition().getZ();
  166.         if (!isOnlyPosition()) {
  167.             value[row + 3] = pv.getVelocity().getX();
  168.             value[row + 4] = pv.getVelocity().getY();
  169.             value[row + 5] = pv.getVelocity().getZ();
  170.         }
  171.     }

  173.     /** Fill up a few Jacobian rows (either 6 or 3 depending on velocities used or not).
  174.      * @param value values vector
  175.      * @param jacobian Jacobian matrix
  176.      * @param row first row index
  177.      * @param state spacecraft state
  178.      * @param stateSize state size
  179.      * @param orbitalParameters drivers for the orbital parameters
  180.      * @param propagationParameters drivers for the propagation model parameters
  181.      * @param mapper state mapper
  182.      */
  183.     private void fillRows(final RealVector value, final RealMatrix jacobian, final int row,
  184.                           final SpacecraftState state, final int stateSize,
  185.                           final ParameterDriversList orbitalParameters,
  186.                           final ParameterDriversList propagationParameters,
  187.                           final JacobiansMapper mapper) {

  189.         // value part
  190.         final PVCoordinates pv = state.getPVCoordinates(getFrame());
  191.         value.setEntry(row,     pv.getPosition().getX());
  192.         value.setEntry(row + 1, pv.getPosition().getY());
  193.         value.setEntry(row + 2, pv.getPosition().getZ());
  194.         if (!isOnlyPosition()) {
  195.             value.setEntry(row + 3, pv.getVelocity().getX());
  196.             value.setEntry(row + 4, pv.getVelocity().getY());
  197.             value.setEntry(row + 5, pv.getVelocity().getZ());
  198.         }

  200.         // Jacobian part
  201.         final double[][] dYdY0 = new double[JacobiansMapper.STATE_DIMENSION][JacobiansMapper.STATE_DIMENSION];
  202.         final double[][] dYdP  = new double[JacobiansMapper.STATE_DIMENSION][mapper.getParameters()];
  203.         mapper.getStateJacobian(state, dYdY0);
  204.         mapper.getParametersJacobian(state, dYdP);
  205.         for (int k = 0; k < stateSize; k++) {
  206.             int index = 0;
  207.             for (int j = 0; j < orbitalParameters.getNbParams(); ++j) {
  208.                 final ParameterDriver driver = orbitalParameters.getDrivers().get(j);
  209.                 if (driver.isSelected()) {
  210.                     jacobian.setEntry(row + k, index++, dYdY0[k][j] * driver.getScale());
  211.                 }
  212.             }
  213.             for (int j = 0; j < propagationParameters.getNbParams(); ++j) {
  214.                 final ParameterDriver driver = propagationParameters.getDrivers().get(j);
  215.                 jacobian.setEntry(row + k, index++, dYdP[k][j] * driver.getScale());
  216.             }
  217.         }

  219.     }

  221. }
Created with JaCoCo 0.8.6.202009150832