BatchLSEstimator.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.estimation.leastsquares;

  19. import java.util.ArrayList;
  20. import java.util.Arrays;
  21. import java.util.Collections;
  22. import java.util.Comparator;
  23. import java.util.List;
  24. import java.util.Map;

  26. import org.hipparchus.exception.LocalizedCoreFormats;
  27. import org.hipparchus.exception.MathIllegalArgumentException;
  28. import org.hipparchus.exception.MathRuntimeException;
  29. import org.hipparchus.linear.RealMatrix;
  30. import org.hipparchus.linear.RealVector;
  31. import org.hipparchus.optim.ConvergenceChecker;
  32. import org.hipparchus.optim.nonlinear.vector.leastsquares.EvaluationRmsChecker;
  33. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresBuilder;
  34. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer;
  35. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer.Optimum;
  36. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem;
  37. import org.hipparchus.optim.nonlinear.vector.leastsquares.ParameterValidator;
  38. import org.hipparchus.util.Incrementor;
  39. import org.orekit.errors.OrekitException;
  40. import org.orekit.estimation.measurements.EstimatedMeasurement;
  41. import org.orekit.estimation.measurements.EstimationsProvider;
  42. import org.orekit.estimation.measurements.ObservedMeasurement;
  43. import org.orekit.orbits.Orbit;
  44. import org.orekit.propagation.Propagator;
  45. import org.orekit.propagation.analytical.BrouwerLyddanePropagator;
  46. import org.orekit.propagation.analytical.EcksteinHechlerPropagator;
  47. import org.orekit.propagation.analytical.Ephemeris;
  48. import org.orekit.propagation.analytical.KeplerianPropagator;
  49. import org.orekit.propagation.analytical.tle.TLEPropagator;
  50. import org.orekit.propagation.conversion.AbstractPropagatorBuilder;
  51. import org.orekit.propagation.conversion.PropagatorBuilder;
  52. import org.orekit.propagation.numerical.NumericalPropagator;
  53. import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
  54. import org.orekit.time.AbsoluteDate;
  55. import org.orekit.utils.ParameterDriver;
  56. import org.orekit.utils.ParameterDriversList;
  57. import org.orekit.utils.ParameterDriversList.DelegatingDriver;
  58. import org.orekit.utils.TimeSpanMap.Span;


  61. /** Least squares estimator for orbit determination.
  62.  * <p>
  63.  * The least squares estimator can be used with different orbit propagators
  64.  * in Orekit. Current propagators list of usable propagators are {@link NumericalPropagator numerical},
  65.  * {@link DSSTPropagator DSST}, {@link BrouwerLyddanePropagator Brouwer-Lyddane},
  66.  * {@link EcksteinHechlerPropagator Eckstein-Hechler}, {@link TLEPropagator SGP4},
  67.  * {@link KeplerianPropagator Keplerian}, and {@link Ephemeris ephemeris-based}.
  68.  * </p>
  69.  * @author Luc Maisonobe
  70.  * @since 8.0
  71.  */
  72. public class BatchLSEstimator {

  74.     /** Builders for propagator. */
  75.     private final PropagatorBuilder[] builders;

  77.     /** Measurements. */
  78.     private final List<ObservedMeasurement<?>> measurements;

  80.     /** Solver for least squares problem. */
  81.     private final LeastSquaresOptimizer optimizer;

  83.     /** Convergence checker. */
  84.     private ConvergenceChecker<LeastSquaresProblem.Evaluation> convergenceChecker;

  86.     /** Builder for the least squares problem. */
  87.     private final LeastSquaresBuilder lsBuilder;

  89.     /** Observer for iterations. */
  90.     private BatchLSObserver observer;

  92.     /** Last estimations. */
  93.     private Map<ObservedMeasurement<?>, EstimatedMeasurement<?>> estimations;

  95.     /** Last orbits. */
  96.     private Orbit[] orbits;

  98.     /** Optimum found. */
  99.     private Optimum optimum;

  101.     /** Counter for the evaluations. */
  102.     private Incrementor evaluationsCounter;

  104.     /** Counter for the iterations. */
  105.     private Incrementor iterationsCounter;

  107.     /** Simple constructor.
  108.      * <p>
  109.      * If multiple {@link PropagatorBuilder propagator builders} are set up,
  110.      * the orbits of several spacecrafts will be used simultaneously.
  111.      * This is useful if the propagators share some model or measurements
  112.      * parameters (typically pole motion, prime meridian correction or
  113.      * ground stations positions).
  114.      * </p>
  115.      * <p>
  116.      * Setting up multiple {@link PropagatorBuilder propagator builders} is
  117.      * also useful when inter-satellite measurements are used, even if only one
  118.      * of the orbit is estimated and the other ones are fixed. This is typically
  119.      * used when very high accuracy GNSS measurements are needed and the
  120.      * navigation bulletins are not considered accurate enough and the navigation
  121.      * constellation must be propagated numerically.
  122.      * </p>
  123.      * @param optimizer solver for least squares problem
  124.      * @param propagatorBuilder builders to use for propagation
  125.      */
  126.     public BatchLSEstimator(final LeastSquaresOptimizer optimizer,
  127.                             final PropagatorBuilder... propagatorBuilder) {

  129.         this.builders                       = propagatorBuilder;
  130.         this.measurements                   = new ArrayList<>();
  131.         this.optimizer                      = optimizer;
  132.         this.lsBuilder                      = new LeastSquaresBuilder();
  133.         this.observer                       = null;
  134.         this.estimations                    = null;
  135.         this.orbits                         = new Orbit[builders.length];

  137.         setParametersConvergenceThreshold(Double.NaN);

  139.         // our model computes value and Jacobian in one call,
  140.         // so we don't use the lazy evaluation feature
  141.         lsBuilder.lazyEvaluation(false);

  143.         // we manage weight by ourselves, as we change them during
  144.         // iterations (setting to 0 the identified outliers measurements)
  145.         // so the least squares problem should not see our weights
  146.         lsBuilder.weight(null);

  148.     }

  150.     /** Set an observer for iterations.
  151.      * @param observer observer to be notified at the end of each iteration
  152.      */
  153.     public void setObserver(final BatchLSObserver observer) {
  154.         this.observer = observer;
  155.     }

  157.     /** Add a measurement.
  158.      * @param measurement measurement to add
  159.      */
  160.     public void addMeasurement(final ObservedMeasurement<?> measurement) {
  161.         measurements.add(measurement);
  162.     }

  164.     /** Set the maximum number of iterations.
  165.      * <p>
  166.      * The iterations correspond to the top level iterations of
  167.      * the {@link LeastSquaresOptimizer least squares optimizer}.
  168.      * </p>
  169.      * @param maxIterations maxIterations maximum number of iterations
  170.      * @see #setMaxEvaluations(int)
  171.      * @see #getIterationsCount()
  172.      */
  173.     public void setMaxIterations(final int maxIterations) {
  174.         lsBuilder.maxIterations(maxIterations);
  175.     }

  177.     /** Set the maximum number of model evaluations.
  178.      * <p>
  179.      * The evaluations correspond to the orbit propagations and
  180.      * measurements estimations performed with a set of estimated
  181.      * parameters.
  182.      * </p>
  183.      * <p>
  184.      * For {@link org.hipparchus.optim.nonlinear.vector.leastsquares.GaussNewtonOptimizer
  185.      * Gauss-Newton optimizer} there is one evaluation at each iteration,
  186.      * so the maximum numbers may be set to the same value. For {@link
  187.      * org.hipparchus.optim.nonlinear.vector.leastsquares.LevenbergMarquardtOptimizer
  188.      * Levenberg-Marquardt optimizer}, there can be several evaluations at
  189.      * some iterations (typically for the first couple of iterations), so the
  190.      * maximum number of evaluations may be set to a higher value than the
  191.      * maximum number of iterations.
  192.      * </p>
  193.      * @param maxEvaluations maximum number of model evaluations
  194.      * @see #setMaxIterations(int)
  195.      * @see #getEvaluationsCount()
  196.      */
  197.     public void setMaxEvaluations(final int maxEvaluations) {
  198.         lsBuilder.maxEvaluations(maxEvaluations);
  199.     }

  201.     /** Get the orbital parameters supported by this estimator.
  202.      * <p>
  203.      * If there are more than one propagator builder, then the names
  204.      * of the drivers have an index marker in square brackets appended
  205.      * to them in order to distinguish the various orbits. So for example
  206.      * with one builder generating Keplerian orbits the names would be
  207.      * simply "a", "e", "i"... but if there are several builders the
  208.      * names would be "a[0]", "e[0]", "i[0]"..."a[1]", "e[1]", "i[1]"...
  209.      * </p>
  210.      * @param estimatedOnly if true, only estimated parameters are returned
  211.      * @return orbital parameters supported by this estimator
  212.      */
  213.     public ParameterDriversList getOrbitalParametersDrivers(final boolean estimatedOnly) {

  215.         final ParameterDriversList estimated = new ParameterDriversList();
  216.         for (int i = 0; i < builders.length; ++i) {
  217.             final String suffix = builders.length > 1 ? "[" + i + "]" : null;
  218.             for (final DelegatingDriver delegating : builders[i].getOrbitalParametersDrivers().getDrivers()) {
  219.                 if (delegating.isSelected() || !estimatedOnly) {
  220.                     for (final ParameterDriver driver : delegating.getRawDrivers()) {
  221.                         if (suffix != null && !driver.getName().endsWith(suffix)) {
  222.                             // we add suffix only conditionally because the method may already have been called
  223.                             // and suffixes may have already been appended
  224.                             driver.setName(driver.getName() + suffix);
  225.                         }
  226.                         estimated.add(driver);
  227.                     }
  228.                 }
  229.             }
  230.         }
  231.         return estimated;

  233.     }

  235.     /** Get the propagator parameters supported by this estimator.
  236.      * @param estimatedOnly if true, only estimated parameters are returned
  237.      * @return propagator parameters supported by this estimator
  238.      */
  239.     public ParameterDriversList getPropagatorParametersDrivers(final boolean estimatedOnly) {

  241.         final ParameterDriversList estimated = new ParameterDriversList();
  242.         for (PropagatorBuilder builder : builders) {
  243.             for (final DelegatingDriver delegating : builder.getPropagationParametersDrivers().getDrivers()) {
  244.                 if (delegating.isSelected() || !estimatedOnly) {
  245.                     for (final ParameterDriver driver : delegating.getRawDrivers()) {
  246.                         estimated.add(driver);
  247.                     }
  248.                 }
  249.             }
  250.         }
  251.         return estimated;

  253.     }

  255.     /** Get the measurements parameters supported by this estimator (including measurements and modifiers).
  256.      * @param estimatedOnly if true, only estimated parameters are returned
  257.      * @return measurements parameters supported by this estimator
  258.      */
  259.     public ParameterDriversList getMeasurementsParametersDrivers(final boolean estimatedOnly) {

  261.         final ParameterDriversList parameters =  new ParameterDriversList();
  262.         for (final  ObservedMeasurement<?> measurement : measurements) {
  263.             for (final ParameterDriver driver : measurement.getParametersDrivers()) {
  264.                 if (!estimatedOnly || driver.isSelected()) {
  265.                     parameters.add(driver);
  266.                 }
  267.             }
  268.         }

  270.         parameters.sort();

  272.         return parameters;

  274.     }

  276.     /**
  277.      * Set convergence threshold.
  278.      * <p>
  279.      * The convergence used for estimation is based on the estimated
  280.      * parameters {@link ParameterDriver#getNormalizedValue() normalized values}.
  281.      * Convergence is considered to have been reached when the difference
  282.      * between previous and current normalized value is less than the
  283.      * convergence threshold for all parameters. The same value is used
  284.      * for all parameters since they are normalized and hence dimensionless.
  285.      * </p>
  286.      * <p>
  287.      * Normalized values are computed as {@code (current - reference)/scale},
  288.      * so convergence is reached when the following condition holds for
  289.      * all estimated parameters:
  290.      * {@code |current[i] - previous[i]| <= threshold * scale[i]}
  291.      * </p>
  292.      * <p>
  293.      * So the convergence threshold specified here can be considered as
  294.      * a multiplication factor applied to scale. Since for all parameters
  295.      * the scale is often small (typically about 1 m for orbital positions
  296.      * for example), then the threshold should not be too small. A value
  297.      * of 10⁻³ is often quite accurate.
  298.      * </p>
  299.      * <p>
  300.      * Calling this method overrides any checker that could have been set
  301.      * beforehand by calling {@link #setConvergenceChecker(ConvergenceChecker)}.
  302.      * Both methods are mutually exclusive.
  303.      * </p>
  304.      *
  305.      * @param parametersConvergenceThreshold convergence threshold on
  306.      * normalized parameters (dimensionless, related to parameters scales)
  307.      * @see #setConvergenceChecker(ConvergenceChecker)
  308.      * @see EvaluationRmsChecker
  309.      */
  310.     public void setParametersConvergenceThreshold(final double parametersConvergenceThreshold) {
  311.         setConvergenceChecker((iteration, previous, current) ->
  312.                               current.getPoint().getLInfDistance(previous.getPoint()) <= parametersConvergenceThreshold);
  313.     }

  315.     /** Set a custom convergence checker.
  316.      * <p>
  317.      * Calling this method overrides any checker that could have been set
  318.      * beforehand by calling {@link #setParametersConvergenceThreshold(double)}.
  319.      * Both methods are mutually exclusive.
  320.      * </p>
  321.      * @param convergenceChecker convergence checker to set
  322.      * @see #setParametersConvergenceThreshold(double)
  323.      * @since 10.1
  324.      */
  325.     public void setConvergenceChecker(final ConvergenceChecker<LeastSquaresProblem.Evaluation> convergenceChecker) {
  326.         this.convergenceChecker = convergenceChecker;
  327.     }

  329.     /** Estimate the orbital, propagation and measurements parameters.
  330.      * <p>
  331.      * The initial guess for all parameters must have been set before calling this method
  332.      * using {@link #getOrbitalParametersDrivers(boolean)}, {@link #getPropagatorParametersDrivers(boolean)},
  333.      * and {@link #getMeasurementsParametersDrivers(boolean)} and then {@link ParameterDriver#setValue(double)
  334.      * setting the values} of the parameters.
  335.      * </p>
  336.      * <p>
  337.      * For parameters whose reference date has not been set to a non-null date beforehand (i.e.
  338.      * the parameters for which {@link ParameterDriver#getReferenceDate()} returns {@code null},
  339.      * a default reference date will be set automatically at the start of the estimation to the
  340.      * {@link AbstractPropagatorBuilder#getInitialOrbitDate() initial orbit date} of the first
  341.      * propagator builder. For parameters whose reference date has been set to a non-null date,
  342.      * this reference date is untouched.
  343.      * </p>
  344.      * <p>
  345.      * After this method returns, the estimated parameters can be retrieved using
  346.      * {@link #getOrbitalParametersDrivers(boolean)}, {@link #getPropagatorParametersDrivers(boolean)},
  347.      * and {@link #getMeasurementsParametersDrivers(boolean)} and then {@link ParameterDriver#getValue()
  348.      * getting the values} of the parameters.
  349.      * </p>
  350.      * <p>
  351.      * As a convenience, the method also returns a fully configured and ready to use
  352.      * propagator set up with all the estimated values.
  353.      * </p>
  354.      * <p>
  355.      * For even more in-depth information, the {@link #getOptimum()} method provides detailed
  356.      * elements (covariance matrix, estimated parameters standard deviation, weighted Jacobian, RMS,
  357.      * χ², residuals and more).
  358.      * </p>
  359.      * @return propagators configured with estimated orbits as initial states, and all
  360.      * propagators estimated parameters also set
  361.      */
  362.     public Propagator[] estimate() {

  364.         // set reference date for all parameters that lack one (including the not estimated parameters)
  365.         for (final ParameterDriver driver : getOrbitalParametersDrivers(false).getDrivers()) {
  366.             if (driver.getReferenceDate() == null) {
  367.                 driver.setReferenceDate(builders[0].getInitialOrbitDate());
  368.             }
  369.         }
  370.         for (final ParameterDriver driver : getPropagatorParametersDrivers(false).getDrivers()) {
  371.             if (driver.getReferenceDate() == null) {
  372.                 driver.setReferenceDate(builders[0].getInitialOrbitDate());
  373.             }
  374.         }
  375.         for (final ParameterDriver driver : getMeasurementsParametersDrivers(false).getDrivers()) {
  376.             if (driver.getReferenceDate() == null) {
  377.                 driver.setReferenceDate(builders[0].getInitialOrbitDate());
  378.             }
  379.         }

  381.         // get all estimated parameters
  382.         final ParameterDriversList estimatedOrbitalParameters      = getOrbitalParametersDrivers(true);
  383.         final ParameterDriversList estimatedPropagatorParameters   = getPropagatorParametersDrivers(true);
  384.         final ParameterDriversList estimatedMeasurementsParameters = getMeasurementsParametersDrivers(true);

  386.         // create start point
  387.         final double[] start = new double[estimatedOrbitalParameters.getNbValuesToEstimate() +
  388.                                           estimatedPropagatorParameters.getNbValuesToEstimate() +
  389.                                           estimatedMeasurementsParameters.getNbValuesToEstimate()];

  391.         int iStart = 0;
  392.         for (final ParameterDriver driver : estimatedOrbitalParameters.getDrivers()) {
  393.             Span<Double> span = driver.getValueSpanMap().getFirstSpan();
  394.             start[iStart++] = driver.getNormalizedValue(span.getStart());
  395.             for (int spanNumber = 0; spanNumber < driver.getNbOfValues() - 1; ++spanNumber) {
  396.                 span = driver.getValueSpanMap().getSpan(span.getEnd());
  397.                 start[iStart++] = driver.getNormalizedValue(span.getStart());
  398.             }
  399.         }
  400.         for (final ParameterDriver driver : estimatedPropagatorParameters.getDrivers()) {
  401.             Span<Double> span = driver.getValueSpanMap().getFirstSpan();
  402.             start[iStart++] = driver.getNormalizedValue(span.getStart());
  403.             for (int spanNumber = 0; spanNumber < driver.getNbOfValues() - 1; ++spanNumber) {
  404.                 span = driver.getValueSpanMap().getSpan(span.getEnd());
  405.                 start[iStart++] = driver.getNormalizedValue(span.getStart());
  406.             }
  407.         }
  408.         for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
  409.             Span<Double> span = driver.getValueSpanMap().getFirstSpan();
  410.             start[iStart++] = driver.getNormalizedValue(span.getStart());
  411.             for (int spanNumber = 0; spanNumber < driver.getNbOfValues() - 1; ++spanNumber) {
  412.                 span = driver.getValueSpanMap().getSpan(span.getEnd());
  413.                 start[iStart++] = driver.getNormalizedValue(span.getStart());
  414.             }
  415.         }
  416.         lsBuilder.start(start);

  418.         // create target (which is an array set to 0, as we compute weighted residuals ourselves)
  419.         int p = 0;
  420.         for (final ObservedMeasurement<?> measurement : measurements) {
  421.             if (measurement.isEnabled()) {
  422.                 p += measurement.getDimension();
  423.             }
  424.         }
  425.         final double[] target = new double[p];
  426.         lsBuilder.target(target);

  428.         // set up the model
  429.         final ModelObserver modelObserver = new ModelObserver() {
  430.             /** {@inheritDoc} */
  431.             @Override
  432.             public void modelCalled(final Orbit[] newOrbits,
  433.                                     final Map<ObservedMeasurement<?>, EstimatedMeasurement<?>> newEstimations) {
  434.                 BatchLSEstimator.this.orbits      = newOrbits;
  435.                 BatchLSEstimator.this.estimations = newEstimations;
  436.             }
  437.         };
  438.         final AbstractBatchLSModel model = builders[0].buildLeastSquaresModel(builders, measurements, estimatedMeasurementsParameters, modelObserver);

  440.         lsBuilder.model(model);

  442.         // add a validator for orbital parameters
  443.         lsBuilder.parameterValidator(new Validator(estimatedOrbitalParameters,
  444.                                                    estimatedPropagatorParameters,
  445.                                                    estimatedMeasurementsParameters));

  447.         lsBuilder.checker(convergenceChecker);

  449.         // set up the problem to solve
  450.         final LeastSquaresProblem problem = new TappedLSProblem(lsBuilder.build(),
  451.                                                                 model,
  452.                                                                 estimatedOrbitalParameters,
  453.                                                                 estimatedPropagatorParameters,
  454.                                                                 estimatedMeasurementsParameters);

  456.         try {

  458.             // solve the problem
  459.             optimum = optimizer.optimize(problem);

  461.             // create a new configured propagator with all estimated parameters
  462.             return model.createPropagators(optimum.getPoint());

  464.         } catch (MathRuntimeException mrte) {
  465.             throw new OrekitException(mrte);
  466.         }
  467.     }

  469.     /** Get the last estimations performed.
  470.      * @return last estimations performed
  471.      */
  472.     public Map<ObservedMeasurement<?>, EstimatedMeasurement<?>> getLastEstimations() {
  473.         return Collections.unmodifiableMap(estimations);
  474.     }

  476.     /** Get the optimum found.
  477.      * <p>
  478.      * The {@link Optimum} object contains detailed elements (covariance matrix, estimated
  479.      * parameters standard deviation, weighted Jacobian, RMS, χ², residuals and more).
  480.      * </p>
  481.      * <p>
  482.      * Beware that the returned object is the raw view from the underlying mathematical
  483.      * library. At this raw level, parameters have {@link ParameterDriver#getNormalizedValue()
  484.      * normalized values} whereas the space flight parameters have {@link ParameterDriver#getValue()
  485.      * physical values} with their units. So there are {@link ParameterDriver#getScale() scaling
  486.      * factors} to apply when using these elements.
  487.      * </p>
  488.      * @return optimum found after last call to {@link #estimate()}
  489.      */
  490.     public Optimum getOptimum() {
  491.         return optimum;
  492.     }

  494.     /** Get the covariances matrix in space flight dynamics physical units.
  495.      * <p>
  496.      * This method retrieve the {@link
  497.      * org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem.Evaluation#getCovariances(double)
  498.      * covariances} from the [@link {@link #getOptimum() optimum} and applies the scaling factors
  499.      * to it in order to convert it from raw normalized values back to physical values.
  500.      * </p>
  501.      * @param threshold threshold to identify matrix singularity
  502.      * @return covariances matrix in space flight dynamics physical units
  503.      * @since 9.1
  504.      */
  505.     public RealMatrix getPhysicalCovariances(final double threshold) {
  506.         final RealMatrix covariances;
  507.         try {
  508.             // get the normalized matrix
  509.             covariances = optimum.getCovariances(threshold).copy();
  510.         } catch (MathIllegalArgumentException miae) {
  511.             // the problem is singular
  512.             throw new OrekitException(miae);
  513.         }

  515.         // retrieve the scaling factors
  516.         final double[] scale = new double[covariances.getRowDimension()];
  517.         int index = 0;
  518.         for (final ParameterDriver driver : getOrbitalParametersDrivers(true).getDrivers()) {
  519.             for (int i = 0; i < driver.getNbOfValues(); ++i) {
  520.                 scale[index++] = driver.getScale();
  521.             }
  522.         }
  523.         for (final ParameterDriver driver : getPropagatorParametersDrivers(true).getDrivers()) {
  524.             for (int i = 0; i < driver.getNbOfValues(); ++i) {
  525.                 scale[index++] = driver.getScale();
  526.             }
  527.         }
  528.         for (final ParameterDriver driver : getMeasurementsParametersDrivers(true).getDrivers()) {
  529.             for (int i = 0; i < driver.getNbOfValues(); ++i) {
  530.                 scale[index++] = driver.getScale();
  531.             }
  532.         }

  534.         // unnormalize the matrix, to retrieve physical covariances
  535.         for (int i = 0; i < covariances.getRowDimension(); ++i) {
  536.             for (int j = 0; j < covariances.getColumnDimension(); ++j) {
  537.                 covariances.setEntry(i, j, scale[i] * scale[j] * covariances.getEntry(i, j));
  538.             }
  539.         }

  541.         return covariances;

  543.     }

  545.     /** Get the number of iterations used for last estimation.
  546.      * @return number of iterations used for last estimation
  547.      * @see #setMaxIterations(int)
  548.      */
  549.     public int getIterationsCount() {
  550.         return iterationsCounter.getCount();
  551.     }

  553.     /** Get the number of evaluations used for last estimation.
  554.      * @return number of evaluations used for last estimation
  555.      * @see #setMaxEvaluations(int)
  556.      */
  557.     public int getEvaluationsCount() {
  558.         return evaluationsCounter.getCount();
  559.     }

  561.     /** Wrapper used to tap the various counters. */
  562.     private class TappedLSProblem implements LeastSquaresProblem {

  564.         /** Underlying problem. */
  565.         private final LeastSquaresProblem problem;

  567.         /** Multivariate function model. */
  568.         private final AbstractBatchLSModel model;

  570.         /** Estimated orbital parameters. */
  571.         private final ParameterDriversList estimatedOrbitalParameters;

  573.         /** Estimated propagator parameters. */
  574.         private final ParameterDriversList estimatedPropagatorParameters;

  576.         /** Estimated measurements parameters. */
  577.         private final ParameterDriversList estimatedMeasurementsParameters;

  579.         /** Simple constructor.
  580.          * @param problem underlying problem
  581.          * @param model multivariate function model
  582.          * @param estimatedOrbitalParameters estimated orbital parameters
  583.          * @param estimatedPropagatorParameters estimated propagator parameters
  584.          * @param estimatedMeasurementsParameters estimated measurements parameters
  585.          */
  586.         TappedLSProblem(final LeastSquaresProblem problem,
  587.                         final AbstractBatchLSModel model,
  588.                         final ParameterDriversList estimatedOrbitalParameters,
  589.                         final ParameterDriversList estimatedPropagatorParameters,
  590.                         final ParameterDriversList estimatedMeasurementsParameters) {
  591.             this.problem                         = problem;
  592.             this.model                           = model;
  593.             this.estimatedOrbitalParameters      = estimatedOrbitalParameters;
  594.             this.estimatedPropagatorParameters   = estimatedPropagatorParameters;
  595.             this.estimatedMeasurementsParameters = estimatedMeasurementsParameters;
  596.         }

  598.         /** {@inheritDoc} */
  599.         @Override
  600.         public Incrementor getEvaluationCounter() {
  601.             // tap the evaluations counter
  602.             BatchLSEstimator.this.evaluationsCounter = problem.getEvaluationCounter();
  603.             model.setEvaluationsCounter(BatchLSEstimator.this.evaluationsCounter);
  604.             return BatchLSEstimator.this.evaluationsCounter;
  605.         }

  607.         /** {@inheritDoc} */
  608.         @Override
  609.         public Incrementor getIterationCounter() {
  610.             // tap the iterations counter
  611.             BatchLSEstimator.this.iterationsCounter = problem.getIterationCounter();
  612.             model.setIterationsCounter(BatchLSEstimator.this.iterationsCounter);
  613.             return BatchLSEstimator.this.iterationsCounter;
  614.         }

  616.         /** {@inheritDoc} */
  617.         @Override
  618.         public ConvergenceChecker<Evaluation> getConvergenceChecker() {
  619.             return problem.getConvergenceChecker();
  620.         }

  622.         /** {@inheritDoc} */
  623.         @Override
  624.         public RealVector getStart() {
  625.             return problem.getStart();
  626.         }

  628.         /** {@inheritDoc} */
  629.         @Override
  630.         public int getObservationSize() {
  631.             return problem.getObservationSize();
  632.         }

  634.         /** {@inheritDoc} */
  635.         @Override
  636.         public int getParameterSize() {
  637.             return problem.getParameterSize();
  638.         }

  640.         /** {@inheritDoc} */
  641.         @Override
  642.         public Evaluation evaluate(final RealVector point) {

  644.             // perform the evaluation
  645.             final Evaluation evaluation = problem.evaluate(point);

  647.             // notify the observer
  648.             if (observer != null) {
  649.                 observer.evaluationPerformed(iterationsCounter.getCount(),
  650.                                              evaluationsCounter.getCount(),
  651.                                              orbits,
  652.                                              estimatedOrbitalParameters,
  653.                                              estimatedPropagatorParameters,
  654.                                              estimatedMeasurementsParameters,
  655.                                              new Provider(),
  656.                                              evaluation);
  657.             }

  659.             return evaluation;

  661.         }

  663.     }

  665.     /** Provider for evaluations. */
  666.     private class Provider implements EstimationsProvider {

  668.         /** Sorted estimations. */
  669.         private EstimatedMeasurement<?>[] sortedEstimations;

  671.         /** {@inheritDoc} */
  672.         @Override
  673.         public int getNumber() {
  674.             return estimations.size();
  675.         }

  677.         /** {@inheritDoc} */
  678.         @Override
  679.         public EstimatedMeasurement<?> getEstimatedMeasurement(final int index) {

  681.             // safety checks
  682.             if (index < 0 || index >= estimations.size()) {
  683.                 throw new OrekitException(LocalizedCoreFormats.OUT_OF_RANGE_SIMPLE,
  684.                                           index, 0, estimations.size());
  685.             }

  687.             if (sortedEstimations == null) {

  689.                 // lazy evaluation of the sorted array
  690.                 sortedEstimations = new EstimatedMeasurement<?>[estimations.size()];
  691.                 int i = 0;
  692.                 for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry : estimations.entrySet()) {
  693.                     sortedEstimations[i++] = entry.getValue();
  694.                 }

  696.                 // sort the array, primarily chronologically
  697.                 Arrays.sort(sortedEstimations, 0, sortedEstimations.length, Comparator.naturalOrder());

  699.             }

  701.             return sortedEstimations[index];

  703.         }

  705.     }

  707.     /** Validator for estimated parameters. */
  708.     private static class Validator implements ParameterValidator {

  710.         /** Estimated orbital parameters. */
  711.         private final ParameterDriversList estimatedOrbitalParameters;

  713.         /** Estimated propagator parameters. */
  714.         private final ParameterDriversList estimatedPropagatorParameters;

  716.         /** Estimated measurements parameters. */
  717.         private final ParameterDriversList estimatedMeasurementsParameters;

  719.         /** Simple constructor.
  720.          * @param estimatedOrbitalParameters estimated orbital parameters
  721.          * @param estimatedPropagatorParameters estimated propagator parameters
  722.          * @param estimatedMeasurementsParameters estimated measurements parameters
  723.          */
  724.         Validator(final ParameterDriversList estimatedOrbitalParameters,
  725.                   final ParameterDriversList estimatedPropagatorParameters,
  726.                   final ParameterDriversList estimatedMeasurementsParameters) {
  727.             this.estimatedOrbitalParameters      = estimatedOrbitalParameters;
  728.             this.estimatedPropagatorParameters   = estimatedPropagatorParameters;
  729.             this.estimatedMeasurementsParameters = estimatedMeasurementsParameters;
  730.         }

  732.         /** {@inheritDoc} */
  733.         @Override
  734.         public RealVector validate(final RealVector params) {

  736.             int i = 0;
  737.             for (final ParameterDriver driver : estimatedOrbitalParameters.getDrivers()) {
  738.                 // let the parameter handle min/max clipping
  739.                 if (driver.getNbOfValues() == 1) {
  740.                     driver.setNormalizedValue(params.getEntry(i), null);
  741.                     params.setEntry(i++, driver.getNormalizedValue(null));

  743.                 // If the parameter driver contains only 1 value to estimate over the all time range
  744.                 } else {
  745.                     // initialization getting the value of the first Span
  746.                     Span<Double> span = driver.getValueSpanMap().getFirstSpan();
  747.                     driver.setNormalizedValue(params.getEntry(i), span.getStart());
  748.                     params.setEntry(i++, driver.getNormalizedValue(span.getStart()));

  750.                     for (int spanNumber = 0; spanNumber < driver.getNbOfValues() - 1; ++spanNumber) {
  751.                         final AbsoluteDate modificationDate = span.getEnd();
  752.                         // get next span, previousSpan.getEnd = span.getStart
  753.                         span = driver.getValueSpanMap().getSpan(modificationDate);
  754.                         driver.setNormalizedValue(params.getEntry(i), modificationDate);
  755.                         params.setEntry(i++, driver.getNormalizedValue(modificationDate));
  756.                     }
  757.                 }

  759.             }
  760.             for (final ParameterDriver driver : estimatedPropagatorParameters.getDrivers()) {
  761.                 // let the parameter handle min/max clipping
  762.                 if (driver.getNbOfValues() == 1) {
  763.                     driver.setNormalizedValue(params.getEntry(i), null);
  764.                     params.setEntry(i++, driver.getNormalizedValue(null));

  766.                 // If the parameter driver contains only 1 value to estimate over the all time range
  767.                 } else {
  768.                     // initialization getting the value of the first Span
  769.                     Span<Double> span = driver.getValueSpanMap().getFirstSpan();
  770.                     driver.setNormalizedValue(params.getEntry(i), span.getStart());
  771.                     params.setEntry(i++, driver.getNormalizedValue(span.getStart()));

  773.                     for (int spanNumber = 0; spanNumber < driver.getNbOfValues() - 1; ++spanNumber) {
  774.                         final AbsoluteDate modificationDate = span.getEnd();
  775.                         // get next span, previousSpan.getEnd = span.getStart
  776.                         span = driver.getValueSpanMap().getSpan(modificationDate);
  777.                         driver.setNormalizedValue(params.getEntry(i), modificationDate);
  778.                         params.setEntry(i++, driver.getNormalizedValue(modificationDate));
  779.                     }
  780.                 }
  781.             }
  782.             for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
  783.                 // let the parameter handle min/max clipping
  784.                 if (driver.getNbOfValues() == 1) {
  785.                     driver.setNormalizedValue(params.getEntry(i), null);
  786.                     params.setEntry(i++, driver.getNormalizedValue(null));

  788.                 // If the parameter driver contains only 1 value to estimate over the all time range
  789.                 } else {
  790.                     // initialization getting the value of the first Span
  791.                     Span<Double> span = driver.getValueSpanMap().getFirstSpan();
  792.                     driver.setNormalizedValue(params.getEntry(i), span.getStart());
  793.                     params.setEntry(i++, driver.getNormalizedValue(span.getStart()));

  795.                     for (int spanNumber = 0; spanNumber < driver.getNbOfValues() - 1; ++spanNumber) {
  796.                         final AbsoluteDate modificationDate = span.getEnd();
  797.                         // get next span, previousSpan.getEnd = span.getStart
  798.                         span = driver.getValueSpanMap().getSpan(modificationDate);
  799.                         driver.setNormalizedValue(params.getEntry(i), modificationDate);
  800.                         params.setEntry(i++, driver.getNormalizedValue(modificationDate));
  801.                     }
  802.                 }
  803.             }
  804.             return params;
  805.         }
  806.     }

  808. }
Created with JaCoCo 0.8.12.202403310830