FiniteDifferencePropagatorConverter.java

  1. /* Copyright 2002-2017 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;

  18. import org.hipparchus.analysis.MultivariateVectorFunction;
  19. import org.hipparchus.linear.MatrixUtils;
  20. import org.hipparchus.linear.RealMatrix;
  21. import org.hipparchus.linear.RealVector;
  22. import org.hipparchus.optim.nonlinear.vector.leastsquares.MultivariateJacobianFunction;
  23. import org.hipparchus.util.Pair;
  24. import org.orekit.errors.OrekitException;
  25. import org.orekit.errors.OrekitExceptionWrapper;
  26. import org.orekit.propagation.Propagator;
  27. import org.orekit.propagation.SpacecraftState;
  28. import org.orekit.utils.PVCoordinates;
  29. import org.orekit.utils.ParameterDriver;

  30. /** Propagator converter using finite differences to compute the Jacobian.
  31.  * @author Pascal Parraud
  32.  * @since 6.0
  33.  */
  34. public class FiniteDifferencePropagatorConverter extends AbstractPropagatorConverter {

  35.     /** Propagator builder. */
  36.     private final PropagatorBuilder builder;

  37.     /** Simple constructor.
  38.      * @param factory builder for adapted propagator
  39.      * @param threshold absolute threshold for optimization algorithm
  40.      * @param maxIterations maximum number of iterations for fitting
  41.      */
  42.     public FiniteDifferencePropagatorConverter(final PropagatorBuilder factory,
  43.                                                final double threshold,
  44.                                                final int maxIterations) {
  45.         super(factory, threshold, maxIterations);
  46.         this.builder = factory;
  47.     }

  48.     /** {@inheritDoc} */
  49.     protected MultivariateVectorFunction getObjectiveFunction() {
  50.         return new ObjectiveFunction();
  51.     }

  52.     /** {@inheritDoc} */
  53.     protected MultivariateJacobianFunction getModel() {
  54.         return new ObjectiveFunctionJacobian();
  55.     }

  56.     /** Internal class for computing position/velocity at sample points. */
  57.     private class ObjectiveFunction implements MultivariateVectorFunction {

  58.         /** {@inheritDoc} */
  59.         public double[] value(final double[] arg)
  60.             throws IllegalArgumentException, OrekitExceptionWrapper {
  61.             try {
  62.                 final Propagator propagator = builder.buildPropagator(arg);
  63.                 final double[] eval = new double[getTargetSize()];
  64.                 int k = 0;
  65.                 for (SpacecraftState state : getSample()) {
  66.                     final PVCoordinates pv = propagator.getPVCoordinates(state.getDate(), getFrame());
  67.                     if (Double.isNaN(pv.getMomentum().getNorm())) {
  68.                         propagator.getPVCoordinates(state.getDate(), getFrame());
  69.                     }
  70.                     eval[k++] = pv.getPosition().getX();
  71.                     eval[k++] = pv.getPosition().getY();
  72.                     eval[k++] = pv.getPosition().getZ();
  73.                     if (!isOnlyPosition()) {
  74.                         eval[k++] = pv.getVelocity().getX();
  75.                         eval[k++] = pv.getVelocity().getY();
  76.                         eval[k++] = pv.getVelocity().getZ();
  77.                     }
  78.                 }

  79.                 return eval;

  80.             } catch (OrekitException ex) {
  81.                 throw new OrekitExceptionWrapper(ex);
  82.             }
  83.         }
  84.     }

  85.     /** Internal class for computing position/velocity Jacobian at sample points. */
  86.     private class ObjectiveFunctionJacobian implements MultivariateJacobianFunction {

  87.         /** {@inheritDoc} */
  88.         public Pair<RealVector, RealMatrix> value(final RealVector point)
  89.             throws IllegalArgumentException, OrekitExceptionWrapper {

  90.             final double[] arg = point.toArray();
  91.             final MultivariateVectorFunction f = new ObjectiveFunction();

  92.             final double[] increment = new double[arg.length];
  93.             int index = 0;
  94.             for (final ParameterDriver driver : builder.getOrbitalParametersDrivers().getDrivers()) {
  95.                 if (driver.isSelected()) {
  96.                     increment[index++] = driver.getScale();
  97.                 }
  98.             }
  99.             for (final ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {
  100.                 if (driver.isSelected()) {
  101.                     increment[index++] = driver.getScale();
  102.                 }
  103.             }

  104.             final double[][] jacob = new double[getTargetSize()][arg.length];
  105.             final double[] eval = f.value(arg);
  106.             final double[] arg1 = new double[arg.length];
  107.             for (int j = 0; j < arg.length; j++) {
  108.                 System.arraycopy(arg, 0, arg1, 0, arg.length);
  109.                 arg1[j] += increment[j];
  110.                 final double[] eval1 = f.value(arg1);
  111.                 for (int t = 0; t < eval.length; t++) {
  112.                     jacob[t][j] = (eval1[t] - eval[t]) / increment[j];
  113.                 }
  114.             }

  115.             return new Pair<RealVector, RealMatrix>(MatrixUtils.createRealVector(eval),
  116.                                                     MatrixUtils.createRealMatrix(jacob));

  117.         }

  118.     }

  119. }