AbstractPropagatorConverter.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.ArrayList;
  20. import java.util.Arrays;
  21. import java.util.List;

  23. import org.hipparchus.analysis.MultivariateVectorFunction;
  24. import org.hipparchus.exception.MathRuntimeException;
  25. import org.hipparchus.linear.DiagonalMatrix;
  26. import org.hipparchus.optim.ConvergenceChecker;
  27. import org.hipparchus.optim.SimpleVectorValueChecker;
  28. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresBuilder;
  29. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresFactory;
  30. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer;
  31. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem;
  32. import org.hipparchus.optim.nonlinear.vector.leastsquares.LevenbergMarquardtOptimizer;
  33. import org.hipparchus.optim.nonlinear.vector.leastsquares.MultivariateJacobianFunction;
  34. import org.hipparchus.util.FastMath;
  35. import org.orekit.errors.OrekitException;
  36. import org.orekit.errors.OrekitExceptionWrapper;
  37. import org.orekit.errors.OrekitMessages;
  38. import org.orekit.frames.Frame;
  39. import org.orekit.propagation.Propagator;
  40. import org.orekit.propagation.SpacecraftState;
  41. import org.orekit.propagation.integration.AbstractIntegratedPropagator;
  42. import org.orekit.time.AbsoluteDate;
  43. import org.orekit.utils.PVCoordinates;
  44. import org.orekit.utils.ParameterDriver;

  46. /** Common handling of {@link PropagatorConverter} methods for propagators conversions.
  47.  * <p>
  48.  * This abstract class factors the common code for propagators conversion.
  49.  * Only one method must be implemented by derived classes: {@link #getObjectiveFunction()}.
  50.  * </p>
  51.  * <p>
  52.  * The converter uses the LevenbergMarquardtOptimizer from the <a
  53.  * href="https://hipparchus.org/">Hipparchus</a> library.
  54.  * Different implementations correspond to different methods for computing the Jacobian.
  55.  * </p>
  56.  * @author Pascal Parraud
  57.  * @since 6.0
  58.  */
  59. public abstract class AbstractPropagatorConverter implements PropagatorConverter {

  61.     /** Spacecraft states sample. */
  62.     private List<SpacecraftState> sample;

  64.     /** Target position and velocities at sample points. */
  65.     private double[] target;

  67.     /** Weight for residuals. */
  68.     private double[] weight;

  70.     /** Auxiliary outputData: RMS of solution. */
  71.     private double rms;

  73.     /** Position use indicator. */
  74.     private boolean onlyPosition;

  76.     /** Adapted propagator. */
  77.     private Propagator adapted;

  79.     /** Propagator builder. */
  80.     private final PropagatorBuilder builder;

  82.     /** Frame. */
  83.     private final Frame frame;

  85.     /** Optimizer for fitting. */
  86.     private final LevenbergMarquardtOptimizer optimizer;

  88.     /** Optimum found. */
  89.     private LeastSquaresOptimizer.Optimum optimum;

  91.     /** Convergence checker for optimization algorithm. */
  92.     private final ConvergenceChecker<LeastSquaresProblem.Evaluation> checker;

  94.     /** Maximum number of iterations for optimization. */
  95.     private final int maxIterations;

  97.     /** Build a new instance.
  98.      * @param builder propagator builder
  99.      * @param threshold absolute convergence threshold for optimization algorithm
  100.      * @param maxIterations maximum number of iterations for fitting
  101.      */
  102.     protected AbstractPropagatorConverter(final PropagatorBuilder builder,
  103.                                           final double threshold,
  104.                                           final int maxIterations) {
  105.         this.builder       = builder;
  106.         this.frame         = builder.getFrame();
  107.         this.optimizer     = new LevenbergMarquardtOptimizer();
  108.         this.maxIterations = maxIterations;
  109.         this.sample        = new ArrayList<SpacecraftState>();

  111.         final SimpleVectorValueChecker svvc = new SimpleVectorValueChecker(-1.0, threshold);
  112.         this.checker = LeastSquaresFactory.evaluationChecker(svvc);

  114.     }

  116.     /** Convert a propagator to another.
  117.      * @param source initial propagator (the propagator will be used for sample
  118.      * generation, if it is a numerical propagator, its initial state will
  119.      * be reset unless {@link AbstractIntegratedPropagator#setResetAtEnd(boolean)}
  120.      * has been called beforehand)
  121.      * @param timeSpan time span for fitting
  122.      * @param nbPoints number of fitting points over time span
  123.      * @param freeParameters names of the free parameters
  124.      * @return adapted propagator
  125.      * @exception OrekitException if propagator cannot be adapted
  126.      * @exception IllegalArgumentException if one of the parameters cannot be free
  127.      */
  128.     public Propagator convert(final Propagator source,
  129.                               final double timeSpan,
  130.                               final int nbPoints,
  131.                               final List<String> freeParameters)
  132.         throws OrekitException, IllegalArgumentException {
  133.         setFreeParameters(freeParameters);
  134.         final List<SpacecraftState> states = createSample(source, timeSpan, nbPoints);
  135.         return convert(states, false, freeParameters);
  136.     }

  138.     /** Convert a propagator to another.
  139.      * @param source initial propagator (the propagator will be used for sample
  140.      * generation, if it is a numerical propagator, its initial state will
  141.      * be reset unless {@link AbstractIntegratedPropagator#setResetAtEnd(boolean)}
  142.      * has been called beforehand)
  143.      * @param timeSpan time span for fitting
  144.      * @param nbPoints number of fitting points over time span
  145.      * @param freeParameters names of the free parameters
  146.      * @return adapted propagator
  147.      * @exception OrekitException if propagator cannot be adapted
  148.      * @exception IllegalArgumentException if one of the parameters cannot be free
  149.      */
  150.     public Propagator convert(final Propagator source,
  151.                               final double timeSpan,
  152.                               final int nbPoints,
  153.                               final String... freeParameters)
  154.         throws OrekitException, IllegalArgumentException {
  155.         setFreeParameters(Arrays.asList(freeParameters));
  156.         final List<SpacecraftState> states = createSample(source, timeSpan, nbPoints);
  157.         return convert(states, false, freeParameters);
  158.     }

  160.     /** Find the propagator that minimize the mean square error for a sample of {@link SpacecraftState states}.
  161.      * @param states spacecraft states sample to fit
  162.      * @param positionOnly if true, consider only position data otherwise both position and velocity are used
  163.      * @param freeParameters names of the free parameters
  164.      * @return adapted propagator
  165.      * @exception OrekitException if propagator cannot be adapted
  166.      * @exception IllegalArgumentException if one of the parameters cannot be free
  167.      */
  168.     public Propagator convert(final List<SpacecraftState> states,
  169.                               final boolean positionOnly,
  170.                               final List<String> freeParameters)
  171.         throws OrekitException, IllegalArgumentException {
  172.         setFreeParameters(freeParameters);
  173.         return adapt(states, positionOnly);
  174.     }

  176.     /** Find the propagator that minimize the mean square error for a sample of {@link SpacecraftState states}.
  177.      * @param states spacecraft states sample to fit
  178.      * @param positionOnly if true, consider only position data otherwise both position and velocity are used
  179.      * @param freeParameters names of the free parameters
  180.      * @return adapted propagator
  181.      * @exception OrekitException if propagator cannot be adapted
  182.      * @exception IllegalArgumentException if one of the parameters cannot be free
  183.      */
  184.     public Propagator convert(final List<SpacecraftState> states,
  185.                               final boolean positionOnly,
  186.                               final String... freeParameters)
  187.         throws OrekitException, IllegalArgumentException {
  188.         setFreeParameters(Arrays.asList(freeParameters));
  189.         return adapt(states, positionOnly);
  190.     }

  192.     /** Get the adapted propagator.
  193.      * @return adapted propagator
  194.      */
  195.     public Propagator getAdaptedPropagator() {
  196.         return adapted;
  197.     }

  199.     /** Get the Root Mean Square Deviation of the fitting.
  200.      * @return RMSD
  201.      */
  202.     public double getRMS() {
  203.         return rms;
  204.     }

  206.     /** Get the number of objective function evaluations.
  207.      *  @return the number of objective function evaluations.
  208.      */
  209.     public int getEvaluations() {
  210.         return optimum.getEvaluations();
  211.     }

  213.     /** Get the function computing position/velocity at sample points.
  214.      * @return function computing position/velocity at sample points
  215.      */
  216.     protected abstract MultivariateVectorFunction getObjectiveFunction();

  218.     /** Get the Jacobian of the function computing position/velocity at sample points.
  219.      * @return Jacobian of the function computing position/velocity at sample points
  220.      */
  221.     protected abstract MultivariateJacobianFunction getModel();

  223.     /** Check if fitting uses only sample positions.
  224.      * @return true if fitting uses only sample positions
  225.      */
  226.     protected boolean isOnlyPosition() {
  227.         return onlyPosition;
  228.     }

  230.     /** Get the size of the target.
  231.      * @return target size
  232.      */
  233.     protected int getTargetSize() {
  234.         return target.length;
  235.     }

  237.     /** Get the frame of the initial state.
  238.      * @return the orbit frame
  239.      */
  240.     protected Frame getFrame() {
  241.         return frame;
  242.     }

  244.     /** Get the states sample.
  245.      * @return the states sample
  246.      */
  247.     protected List<SpacecraftState> getSample() {
  248.         return sample;
  249.     }

  251.     /** Create a sample of {@link SpacecraftState}.
  252.      * @param source initial propagator
  253.      * @param timeSpan time span for the sample
  254.      * @param nbPoints number of points for the sample over the time span
  255.      * @return a sample of {@link SpacecraftState}
  256.      * @exception OrekitException if one of the sample point cannot be computed
  257.      */
  258.     private List<SpacecraftState> createSample(final Propagator source,
  259.                                                final double timeSpan,
  260.                                                final int nbPoints) throws OrekitException {

  262.         final List<SpacecraftState> states = new ArrayList<SpacecraftState>();

  264.         final double stepSize = timeSpan / (nbPoints - 1);
  265.         final AbsoluteDate iniDate = source.getInitialState().getDate();
  266.         for (double dt = 0; dt < timeSpan; dt += stepSize) {
  267.             states.add(source.propagate(iniDate.shiftedBy(dt)));
  268.         }

  270.         return states;
  271.     }

  273.     /** Free some parameters.
  274.      * @param freeParameters names of the free parameters
  275.      * @exception OrekitException if one of the parameters cannot be free
  276.      */
  277.     private void setFreeParameters(final Iterable<String> freeParameters) throws OrekitException {

  279.         // start by setting all parameters as not estimated
  280.         for (final ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {
  281.             driver.setSelected(false);
  282.         }

  284.         // set only the selected parameters as estimated
  285.         for (final String parameter : freeParameters) {
  286.             boolean found = false;
  287.             for (final ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {
  288.                 if (driver.getName().equals(parameter)) {
  289.                     found = true;
  290.                     driver.setSelected(true);
  291.                     break;
  292.                 }
  293.             }
  294.             if (!found) {
  295.                 // build the list of supported parameters
  296.                 final StringBuilder sBuilder = new StringBuilder();
  297.                 for (final ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {
  298.                     if (sBuilder.length() > 0) {
  299.                         sBuilder.append(", ");
  300.                     }
  301.                     sBuilder.append(driver.getName());
  302.                 }
  303.                 throw new OrekitException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME,
  304.                                           parameter, sBuilder.toString());
  305.             }
  306.         }
  307.     }

  309.     /** Adapt a propagator to minimize the mean square error for a set of {@link SpacecraftState states}.
  310.      * @param states set of spacecraft states to fit
  311.      * @param positionOnly if true, consider only position data otherwise both position and velocity are used
  312.      * @return adapted propagator
  313.      * @exception OrekitException if propagator cannot be adapted
  314.      */
  315.     private Propagator adapt(final List<SpacecraftState> states,
  316.                              final boolean positionOnly) throws OrekitException {

  318.         this.onlyPosition = positionOnly;

  320.         // very rough first guess using osculating parameters of first sample point
  321.         final double[] initial = builder.getSelectedNormalizedParameters();

  323.         // warm-up iterations, using only a few points
  324.         setSample(states.subList(0, onlyPosition ? 2 : 1));
  325.         final double[] intermediate = fit(initial);

  327.         // final search using all points
  328.         setSample(states);
  329.         final double[] result = fit(intermediate);

  331.         rms = getRMS(result);
  332.         adapted = buildAdaptedPropagator(result);

  334.         return adapted;
  335.     }

  337.     /** Find the propagator that minimize the mean square error for a sample of {@link SpacecraftState states}.
  338.      * @param initial initial estimation parameters (position, velocity, free parameters)
  339.      * @return fitted parameters
  340.      * @exception OrekitException if propagator cannot be adapted
  341.      * @exception MathRuntimeException if maximal number of iterations is exceeded
  342.      */
  343.     private double[] fit(final double[] initial)
  344.         throws OrekitException, MathRuntimeException {

  346.         final LeastSquaresProblem problem = new LeastSquaresBuilder().
  347.                                             maxIterations(maxIterations).
  348.                                             maxEvaluations(Integer.MAX_VALUE).
  349.                                             model(getModel()).
  350.                                             target(target).
  351.                                             weight(new DiagonalMatrix(weight)).
  352.                                             start(initial).
  353.                                             checker(checker).
  354.                                             build();

  356.         optimum = optimizer.optimize(problem);
  357.         return optimum.getPoint().toArray();

  359.     }

  361.     /** Get the Root Mean Square Deviation for a given parameters set.
  362.      * @param parameterSet position/velocity parameters set
  363.      * @return RMSD
  364.      * @exception OrekitException if position/velocity cannot be computed at some date
  365.      */
  366.     private double getRMS(final double[] parameterSet) throws OrekitException {
  367.         try {
  368.             final double[] residuals = getObjectiveFunction().value(parameterSet);
  369.             for (int i = 0; i < residuals.length; ++i) {
  370.                 residuals[i] = target[i] - residuals[i];
  371.             }
  372.             double sum2 = 0;
  373.             for (final double residual : residuals) {
  374.                 sum2 += residual * residual;
  375.             }
  376.             return FastMath.sqrt(sum2 / residuals.length);

  378.         } catch (OrekitExceptionWrapper oew) {
  379.             throw oew.getException();
  380.         }
  381.     }

  383.     /** Build the adpated propagator for a given position/velocity(/free) parameters set.
  384.      * @param parameterSet position/velocity(/free) parameters set
  385.      * @return adapted propagator
  386.      * @exception OrekitException if propagator cannot be build
  387.      */
  388.     private Propagator buildAdaptedPropagator(final double[] parameterSet)
  389.         throws OrekitException {
  390.         return builder.buildPropagator(parameterSet);
  391.     }

  393.     /** Set the states sample.
  394.      * @param states spacecraft states sample
  395.      * @exception OrekitException if position/velocity cannot be extracted from sample
  396.      */
  397.     private void setSample(final List<SpacecraftState> states) throws OrekitException {

  399.         this.sample = states;

  401.         if (onlyPosition) {
  402.             target = new double[states.size() * 3];
  403.             weight = new double[states.size() * 3];
  404.         } else {
  405.             target = new double[states.size() * 6];
  406.             weight = new double[states.size() * 6];
  407.         }

  409.         int k = 0;
  410.         for (int i = 0; i < states.size(); i++) {

  412.             final PVCoordinates pv = states.get(i).getPVCoordinates(frame);

  414.             // position
  415.             target[k]   = pv.getPosition().getX();
  416.             weight[k++] = 1;
  417.             target[k]   = pv.getPosition().getY();
  418.             weight[k++] = 1;
  419.             target[k]   = pv.getPosition().getZ();
  420.             weight[k++] = 1;

  422.             // velocity
  423.             if (!onlyPosition) {
  424.                 // velocity weight relative to position
  425.                 final double r2 = pv.getPosition().getNormSq();
  426.                 final double v  = pv.getVelocity().getNorm();
  427.                 final double vWeight = v * r2 / states.get(i).getMu();

  429.                 target[k]   = pv.getVelocity().getX();
  430.                 weight[k++] = vWeight;
  431.                 target[k]   = pv.getVelocity().getY();
  432.                 weight[k++] = vWeight;
  433.                 target[k]   = pv.getVelocity().getZ();
  434.                 weight[k++] = vWeight;
  435.             }

  437.         }

  439.     }

  441. }
Created with JaCoCo 0.7.9.201702052155