AbstractPropagatorConverter.java

  1. /* Copyright 2002-2025 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.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.OrekitMessages;
  37. import org.orekit.frames.Frame;
  38. import org.orekit.propagation.Propagator;
  39. import org.orekit.propagation.SpacecraftState;
  40. import org.orekit.propagation.integration.AbstractIntegratedPropagator;
  41. import org.orekit.time.AbsoluteDate;
  42. import org.orekit.utils.PVCoordinates;
  43. import org.orekit.utils.ParameterDriver;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  113.     }

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

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

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

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

  187.     /** Get the adapted propagator.
  188.      * @return adapted propagator
  189.      */
  190.     public Propagator getAdaptedPropagator() {
  191.         return adapted;
  192.     }

  194.     /** Get the Root Mean Square Deviation of the fitting.
  195.      * @return RMSD
  196.      */
  197.     public double getRMS() {
  198.         return rms;
  199.     }

  201.     /** Get the number of objective function evaluations.
  202.      *  @return the number of objective function evaluations.
  203.      */
  204.     public int getEvaluations() {
  205.         return optimum.getEvaluations();
  206.     }

  208.     /** Get the function computing position/velocity at sample points.
  209.      * @return function computing position/velocity at sample points
  210.      */
  211.     protected abstract MultivariateVectorFunction getObjectiveFunction();

  213.     /** Get the Jacobian of the function computing position/velocity at sample points.
  214.      * @return Jacobian of the function computing position/velocity at sample points
  215.      */
  216.     protected abstract MultivariateJacobianFunction getModel();

  218.     /** Check if fitting uses only sample positions.
  219.      * @return true if fitting uses only sample positions
  220.      */
  221.     protected boolean isOnlyPosition() {
  222.         return onlyPosition;
  223.     }

  225.     /** Get the size of the target.
  226.      * @return target size
  227.      */
  228.     protected int getTargetSize() {
  229.         return target.length;
  230.     }

  232.     /** Get the frame of the initial state.
  233.      * @return the orbit frame
  234.      */
  235.     protected Frame getFrame() {
  236.         return frame;
  237.     }

  239.     /** Get the states sample.
  240.      * @return the states sample
  241.      */
  242.     protected List<SpacecraftState> getSample() {
  243.         return sample;
  244.     }

  246.     /** Create a sample of {@link SpacecraftState}.
  247.      * @param source initial propagator
  248.      * @param timeSpan time span for the sample
  249.      * @param nbPoints number of points for the sample over the time span
  250.      * @return a sample of {@link SpacecraftState}
  251.      */
  252.     private List<SpacecraftState> createSample(final Propagator source,
  253.                                                final double timeSpan,
  254.                                                final int nbPoints) {

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

  258.         final double stepSize = timeSpan / (nbPoints - 1);
  259.         final AbsoluteDate iniDate = source.getInitialState().getDate();
  260.         for (double dt = 0; dt < timeSpan; dt += stepSize) {
  261.             states.add(source.propagate(iniDate.shiftedBy(dt)));
  262.         }

  264.         return states;
  265.     }

  267.     /** Free some parameters.
  268.      * @param freeParameters names of the free parameters
  269.      */
  270.     private void setFreeParameters(final Iterable<String> freeParameters) {

  272.         // start by setting all parameters as not estimated
  273.         for (final ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {
  274.             driver.setSelected(false);
  275.         }

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

  302.     /** Adapt a propagator to minimize the mean square error for a set of {@link SpacecraftState states}.
  303.      * @param states set of spacecraft states to fit
  304.      * @param positionOnly if true, consider only position data otherwise both position and velocity are used
  305.      * @return adapted propagator
  306.      */
  307.     private Propagator adapt(final List<SpacecraftState> states,
  308.                              final boolean positionOnly) {

  310.         this.onlyPosition = positionOnly;

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

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

  319.         // final search using all points
  320.         setSample(states);
  321.         final double[] result = fit(intermediate);

  323.         rms = getRMS(result);
  324.         adapted = buildAdaptedPropagator(result);

  326.         return adapted;
  327.     }

  329.     /** Find the propagator that minimize the mean square error for a sample of {@link SpacecraftState states}.
  330.      * @param initial initial estimation parameters (position, velocity, free parameters)
  331.      * @return fitted parameters
  332.           * @exception MathRuntimeException if maximal number of iterations is exceeded
  333.      */
  334.     private double[] fit(final double[] initial)
  335.         throws MathRuntimeException {

  337.         final LeastSquaresProblem problem = new LeastSquaresBuilder().
  338.                                             maxIterations(maxIterations).
  339.                                             maxEvaluations(Integer.MAX_VALUE).
  340.                                             model(getModel()).
  341.                                             target(target).
  342.                                             weight(new DiagonalMatrix(weight)).
  343.                                             start(initial).
  344.                                             checker(checker).
  345.                                             build();

  347.         optimum = optimizer.optimize(problem);
  348.         return optimum.getPoint().toArray();

  350.     }

  352.     /** Get the Root Mean Square Deviation for a given parameters set.
  353.      * @param parameterSet position/velocity parameters set
  354.      * @return RMSD
  355.      */
  356.     private double getRMS(final double[] parameterSet) {
  357.         final double[] residuals = getObjectiveFunction().value(parameterSet);
  358.         for (int i = 0; i < residuals.length; ++i) {
  359.             residuals[i] = target[i] - residuals[i];
  360.         }
  361.         double sum2 = 0;
  362.         for (final double residual : residuals) {
  363.             sum2 += residual * residual;
  364.         }
  365.         return FastMath.sqrt(sum2 / residuals.length);
  366.     }

  368.     /** Build the adpated propagator for a given position/velocity(/free) parameters set.
  369.      * @param parameterSet position/velocity(/free) parameters set
  370.      * @return adapted propagator
  371.      */
  372.     private Propagator buildAdaptedPropagator(final double[] parameterSet) {
  373.         return builder.buildPropagator(parameterSet);
  374.     }

  376.     /** Set the states sample.
  377.      * @param states spacecraft states sample
  378.      */
  379.     private void setSample(final List<SpacecraftState> states) {

  381.         this.sample = states;

  383.         if (onlyPosition) {
  384.             target = new double[states.size() * 3];
  385.             weight = new double[states.size() * 3];
  386.         } else {
  387.             target = new double[states.size() * 6];
  388.             weight = new double[states.size() * 6];
  389.         }

  391.         int k = 0;
  392.         for (SpacecraftState state : states) {

  394.             final PVCoordinates pv = state.getPVCoordinates(frame);

  396.             // position
  397.             target[k] = pv.getPosition().getX();
  398.             weight[k++] = 1;
  399.             target[k] = pv.getPosition().getY();
  400.             weight[k++] = 1;
  401.             target[k] = pv.getPosition().getZ();
  402.             weight[k++] = 1;

  404.             // velocity
  405.             if (!onlyPosition) {
  406.                 // velocity weight relative to position
  407.                 final double r2 = pv.getPosition().getNormSq();
  408.                 final double v = pv.getVelocity().getNorm();
  409.                 final double vWeight = v * r2 / state.getOrbit().getMu();

  411.                 target[k] = pv.getVelocity().getX();
  412.                 weight[k++] = vWeight;
  413.                 target[k] = pv.getVelocity().getY();
  414.                 weight[k++] = vWeight;
  415.                 target[k] = pv.getVelocity().getZ();
  416.                 weight[k++] = vWeight;
  417.             }

  419.         }

  421.     }

  423. }
Created with JaCoCo 0.8.12.202403310830