JacobianPropagatorConverter.java

  1. /* Copyright 2002-2018 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.OrekitExceptionWrapper;
  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.      * @exception OrekitException if the builder {@link
  57.      * NumericalPropagatorBuilder#getOrbitType() orbit type} is not
  58.      * {@link OrbitType#CARTESIAN}
  59.      */
  60.     public JacobianPropagatorConverter(final NumericalPropagatorBuilder builder,
  61.                                        final double threshold,
  62.                                        final int maxIterations)
  63.         throws OrekitException {
  64.         super(builder, threshold, maxIterations);
  65.         if (builder.getOrbitType() != OrbitType.CARTESIAN) {
  66.             throw new OrekitException(OrekitMessages.ORBIT_TYPE_NOT_ALLOWED,
  67.                                       builder.getOrbitType(), OrbitType.CARTESIAN);
  68.         }
  69.         this.builder = builder;
  70.     }

  72.     /** {@inheritDoc} */
  73.     protected MultivariateVectorFunction getObjectiveFunction() {
  74.         return new MultivariateVectorFunction() {

  76.             /** {@inheritDoc} */
  77.             public double[] value(final double[] arg)
  78.                 throws IllegalArgumentException, OrekitExceptionWrapper {
  79.                 try {
  80.                     final double[] value = new double[getTargetSize()];

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

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

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

  108.                     return value;

  110.                 } catch (OrekitException ex) {
  111.                     throw new OrekitExceptionWrapper(ex);
  112.                 }
  113.             }

  115.         };
  116.     }

  118.     /** {@inheritDoc} */
  119.     protected MultivariateJacobianFunction getModel() {
  120.         return new MultivariateJacobianFunction() {

  122.             /** {@inheritDoc} */
  123.             public Pair<RealVector, RealMatrix> value(final RealVector point)
  124.                 throws IllegalArgumentException, OrekitExceptionWrapper {
  125.                 try {

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

  130.                     final NumericalPropagator prop  = builder.buildPropagator(point.toArray());
  131.                     final int stateSize = isOnlyPosition() ? 3 : 6;
  132.                     final ParameterDriversList orbitalParameters = builder.getOrbitalParametersDrivers();
  133.                     final PartialDerivativesEquations pde = new PartialDerivativesEquations("pde", prop);
  134.                     final ParameterDriversList propagationParameters = pde.getSelectedParameters();
  135.                     prop.setInitialState(pde.setInitialJacobians(prop.getInitialState()));
  136.                     final JacobiansMapper mapper  = pde.getMapper();

  138.                     final List<SpacecraftState> sample = getSample();
  139.                     for (int i = 0; i < sample.size(); ++i) {
  140.                         final int row = i * stateSize;
  141.                         if (prop.getInitialState().getDate().equals(sample.get(i).getDate())) {
  142.                             // use initial state and Jacobians
  143.                             fillRows(value, jacobian, row, prop.getInitialState(), stateSize,
  144.                                      orbitalParameters, propagationParameters, mapper);
  145.                         } else {
  146.                             // use a date detector to pick up state and Jacobians
  147.                             prop.addEventDetector(new DateDetector(sample.get(i).getDate()).withHandler(new EventHandler<DateDetector>() {
  148.                                 /** {@inheritDoc} */
  149.                                 @Override
  150.                                 public Action eventOccurred(final SpacecraftState state, final DateDetector detector,
  151.                                                             final boolean increasing)
  152.                                     throws OrekitException {
  153.                                     fillRows(value, jacobian, row, state, stateSize,
  154.                                              orbitalParameters, propagationParameters, mapper);
  155.                                     return row + stateSize >= getTargetSize() ? Action.STOP : Action.CONTINUE;
  156.                                 }
  157.                             }));
  158.                         }
  159.                     }

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

  163.                     return new Pair<RealVector, RealMatrix>(value, jacobian);

  165.                 } catch (OrekitException ex) {
  166.                     throw new OrekitExceptionWrapper(ex);
  167.                 }
  168.             }

  170.         };
  171.     }

  173.     /** Fill up a few value rows (either 6 or 3 depending on velocities used or not).
  174.      * @param value values array
  175.      * @param row first row index
  176.      * @param state spacecraft state
  177.      * @exception OrekitException if Jacobians matrices cannot be retrieved
  178.      */
  179.     private void fillRows(final double[] value, final int row, final SpacecraftState state)
  180.         throws OrekitException {
  181.         final PVCoordinates pv = state.getPVCoordinates(getFrame());
  182.         value[row    ] = pv.getPosition().getX();
  183.         value[row + 1] = pv.getPosition().getY();
  184.         value[row + 2] = pv.getPosition().getZ();
  185.         if (!isOnlyPosition()) {
  186.             value[row + 3] = pv.getVelocity().getX();
  187.             value[row + 4] = pv.getVelocity().getY();
  188.             value[row + 5] = pv.getVelocity().getZ();
  189.         }
  190.     }

  192.     /** Fill up a few Jacobian rows (either 6 or 3 depending on velocities used or not).
  193.      * @param value values vector
  194.      * @param jacobian Jacobian matrix
  195.      * @param row first row index
  196.      * @param state spacecraft state
  197.      * @param stateSize state size
  198.      * @param orbitalParameters drivers for the orbital parameters
  199.      * @param propagationParameters drivers for the propagation model parameters
  200.      * @param mapper state mapper
  201.      * @exception OrekitException if Jacobians matrices cannot be retrieved
  202.      */
  203.     private void fillRows(final RealVector value, final RealMatrix jacobian, final int row,
  204.                           final SpacecraftState state, final int stateSize,
  205.                           final ParameterDriversList orbitalParameters,
  206.                           final ParameterDriversList propagationParameters,
  207.                           final JacobiansMapper mapper)
  208.         throws OrekitException {

  210.         // value part
  211.         final PVCoordinates pv = state.getPVCoordinates(getFrame());
  212.         value.setEntry(row,     pv.getPosition().getX());
  213.         value.setEntry(row + 1, pv.getPosition().getY());
  214.         value.setEntry(row + 2, pv.getPosition().getZ());
  215.         if (!isOnlyPosition()) {
  216.             value.setEntry(row + 3, pv.getVelocity().getX());
  217.             value.setEntry(row + 4, pv.getVelocity().getY());
  218.             value.setEntry(row + 5, pv.getVelocity().getZ());
  219.         }

  221.         // Jacobian part
  222.         final double[][] dYdY0 = new double[JacobiansMapper.STATE_DIMENSION][JacobiansMapper.STATE_DIMENSION];
  223.         final double[][] dYdP  = new double[JacobiansMapper.STATE_DIMENSION][mapper.getParameters()];
  224.         mapper.getStateJacobian(state, dYdY0);
  225.         mapper.getParametersJacobian(state, dYdP);
  226.         for (int k = 0; k < stateSize; k++) {
  227.             int index = 0;
  228.             for (int j = 0; j < orbitalParameters.getNbParams(); ++j) {
  229.                 final ParameterDriver driver = orbitalParameters.getDrivers().get(j);
  230.                 if (driver.isSelected()) {
  231.                     jacobian.setEntry(row + k, index++, dYdY0[k][j] * driver.getScale());
  232.                 }
  233.             }
  234.             for (int j = 0; j < propagationParameters.getNbParams(); ++j) {
  235.                 final ParameterDriver driver = propagationParameters.getDrivers().get(j);
  236.                 jacobian.setEntry(row + k, index++, dYdP[k][j] * driver.getScale());
  237.             }
  238.         }

  240.     }

  242. }
Created with JaCoCo 0.7.9.201702052155