JacobianPropagatorConverter.java

  1. /* Copyright 2002-2019 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (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.SpacecraftState;
  32. import org.orekit.propagation.events.DateDetector;
  33. import org.orekit.propagation.events.handlers.EventHandler;
  34. import org.orekit.propagation.numerical.JacobiansMapper;
  35. import org.orekit.propagation.numerical.NumericalPropagator;
  36. import org.orekit.propagation.numerical.PartialDerivativesEquations;
  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 new MultivariateVectorFunction() {

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  218.     }

  220. }
Created with JaCoCo 0.8.2.201808211720