/* Copyright 2002-2021 CS GROUP
    * Licensed to CS GROUP (CS) under one or more
    * contributor license agreements.  See the NOTICE file distributed with
    * this work for additional information regarding copyright ownership.
    * CS licenses this file to You under the Apache License, Version 2.0
    * (the "License"); you may not use this file except in compliance with
    * the License.  You may obtain a copy of the License at
    *
    *   http://www.apache.org/licenses/LICENSE-2.0
   *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   */
  package org.orekit.estimation.leastsquares;
  
  import java.util.ArrayList;
  import java.util.Arrays;
  import java.util.Collections;
  import java.util.Comparator;
  import java.util.List;
  import java.util.Map;
  
  import org.hipparchus.exception.LocalizedCoreFormats;
  import org.hipparchus.exception.MathIllegalArgumentException;
  import org.hipparchus.exception.MathRuntimeException;
  import org.hipparchus.linear.RealMatrix;
  import org.hipparchus.linear.RealVector;
  import org.hipparchus.optim.ConvergenceChecker;
  import org.hipparchus.optim.nonlinear.vector.leastsquares.EvaluationRmsChecker;
  import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresBuilder;
  import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer;
  import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer.Optimum;
  import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem;
  import org.hipparchus.optim.nonlinear.vector.leastsquares.ParameterValidator;
  import org.hipparchus.util.Incrementor;
  import org.orekit.errors.OrekitException;
  import org.orekit.estimation.measurements.EstimatedMeasurement;
  import org.orekit.estimation.measurements.EstimationsProvider;
  import org.orekit.estimation.measurements.ObservedMeasurement;
  import org.orekit.orbits.Orbit;
  import org.orekit.propagation.Propagator;
  import org.orekit.propagation.conversion.AbstractPropagatorBuilder;
  import org.orekit.propagation.conversion.OrbitDeterminationPropagatorBuilder;
  import org.orekit.propagation.conversion.PropagatorBuilder;
  import org.orekit.propagation.numerical.NumericalPropagator;
  import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
  import org.orekit.utils.ParameterDriver;
  import org.orekit.utils.ParameterDriversList;
  import org.orekit.utils.ParameterDriversList.DelegatingDriver;
  
  
  /** Least squares estimator for orbit determination.
   * <p>
   * Since 10.0, the least squares estimator can be used with both
   * {@link NumericalPropagator numerical} and {@link DSSTPropagator DSST}
   * orbit propagators.
   * </p>
   * @author Luc Maisonobe
   * @since 8.0
   */
  public class BatchLSEstimator {
  
      /** Builders for propagator. */
      private final OrbitDeterminationPropagatorBuilder[] builders;
  
      /** Measurements. */
      private final List<ObservedMeasurement<?>> measurements;
  
      /** Solver for least squares problem. */
      private final LeastSquaresOptimizer optimizer;
  
      /** Convergence checker. */
      private ConvergenceChecker<LeastSquaresProblem.Evaluation> convergenceChecker;
  
      /** Builder for the least squares problem. */
      private final LeastSquaresBuilder lsBuilder;
  
      /** Observer for iterations. */
      private BatchLSObserver observer;
  
      /** Last estimations. */
      private Map<ObservedMeasurement<?>, EstimatedMeasurement<?>> estimations;
  
      /** Last orbits. */
      private Orbit[] orbits;
  
      /** Optimum found. */
      private Optimum optimum;
  
      /** Counter for the evaluations. */
      private Incrementor evaluationsCounter;
  
      /** Counter for the iterations. */
      private Incrementor iterationsCounter;
  
      /** Simple constructor.
      * <p>
      * If multiple {@link PropagatorBuilder propagator builders} are set up,
      * the orbits of several spacecrafts will be used simultaneously.
      * This is useful if the propagators share some model or measurements
      * parameters (typically pole motion, prime meridian correction or
      * ground stations positions).
      * </p>
      * <p>
      * Setting up multiple {@link PropagatorBuilder propagator builders} is
      * also useful when inter-satellite measurements are used, even if only one
      * of the orbit is estimated and the other ones are fixed. This is typically
      * used when very high accuracy GNSS measurements are needed and the
      * navigation bulletins are not considered accurate enough and the navigation
      * constellation must be propagated numerically.
      * </p>
      * @param optimizer solver for least squares problem
      * @param propagatorBuilder builders to use for propagation
      */
     public BatchLSEstimator(final LeastSquaresOptimizer optimizer,
                             final OrbitDeterminationPropagatorBuilder... propagatorBuilder) {
 
         this.builders                       = propagatorBuilder;
         this.measurements                   = new ArrayList<ObservedMeasurement<?>>();
         this.optimizer                      = optimizer;
         this.lsBuilder                      = new LeastSquaresBuilder();
         this.observer                       = null;
         this.estimations                    = null;
         this.orbits                         = new Orbit[builders.length];
 
         setParametersConvergenceThreshold(Double.NaN);
 
         // our model computes value and Jacobian in one call,
         // so we don't use the lazy evaluation feature
         lsBuilder.lazyEvaluation(false);
 
         // we manage weight by ourselves, as we change them during
         // iterations (setting to 0 the identified outliers measurements)
         // so the least squares problem should not see our weights
         lsBuilder.weight(null);
 
     }
 
     /** Set an observer for iterations.
      * @param observer observer to be notified at the end of each iteration
      */
     public void setObserver(final BatchLSObserver observer) {
         this.observer = observer;
     }
 
     /** Add a measurement.
      * @param measurement measurement to add
      */
     public void addMeasurement(final ObservedMeasurement<?> measurement) {
         measurements.add(measurement);
     }
 
     /** Set the maximum number of iterations.
      * <p>
      * The iterations correspond to the top level iterations of
      * the {@link LeastSquaresOptimizer least squares optimizer}.
      * </p>
      * @param maxIterations maxIterations maximum number of iterations
      * @see #setMaxEvaluations(int)
      * @see #getIterationsCount()
      */
     public void setMaxIterations(final int maxIterations) {
         lsBuilder.maxIterations(maxIterations);
     }
 
     /** Set the maximum number of model evaluations.
      * <p>
      * The evaluations correspond to the orbit propagations and
      * measurements estimations performed with a set of estimated
      * parameters.
      * </p>
      * <p>
      * For {@link org.hipparchus.optim.nonlinear.vector.leastsquares.GaussNewtonOptimizer
      * Gauss-Newton optimizer} there is one evaluation at each iteration,
      * so the maximum numbers may be set to the same value. For {@link
      * org.hipparchus.optim.nonlinear.vector.leastsquares.LevenbergMarquardtOptimizer
      * Levenberg-Marquardt optimizer}, there can be several evaluations at
      * some iterations (typically for the first couple of iterations), so the
      * maximum number of evaluations may be set to a higher value than the
      * maximum number of iterations.
      * </p>
      * @param maxEvaluations maximum number of model evaluations
      * @see #setMaxIterations(int)
      * @see #getEvaluationsCount()
      */
     public void setMaxEvaluations(final int maxEvaluations) {
         lsBuilder.maxEvaluations(maxEvaluations);
     }
 
     /** Get the orbital parameters supported by this estimator.
      * <p>
      * If there are more than one propagator builder, then the names
      * of the drivers have an index marker in square brackets appended
      * to them in order to distinguish the various orbits. So for example
      * with one builder generating Keplerian orbits the names would be
      * simply "a", "e", "i"... but if there are several builders the
      * names would be "a[0]", "e[0]", "i[0]"..."a[1]", "e[1]", "i[1]"...
      * </p>
      * @param estimatedOnly if true, only estimated parameters are returned
      * @return orbital parameters supported by this estimator
      */
     public ParameterDriversList getOrbitalParametersDrivers(final boolean estimatedOnly) {
 
         final ParameterDriversList estimated = new ParameterDriversList();
         for (int i = 0; i < builders.length; ++i) {
             final String suffix = builders.length > 1 ? "[" + i + "]" : null;
             for (final DelegatingDriver delegating : builders[i].getOrbitalParametersDrivers().getDrivers()) {
                 if (delegating.isSelected() || !estimatedOnly) {
                     for (final ParameterDriver driver : delegating.getRawDrivers()) {
                         if (suffix != null && !driver.getName().endsWith(suffix)) {
                             // we add suffix only conditionally because the method may already have been called
                             // and suffixes may have already been appended
                             driver.setName(driver.getName() + suffix);
                         }
                         estimated.add(driver);
                     }
                 }
             }
         }
         return estimated;
 
     }
 
     /** Get the propagator parameters supported by this estimator.
      * @param estimatedOnly if true, only estimated parameters are returned
      * @return propagator parameters supported by this estimator
      */
     public ParameterDriversList getPropagatorParametersDrivers(final boolean estimatedOnly) {
 
         final ParameterDriversList estimated = new ParameterDriversList();
         for (PropagatorBuilder builder : builders) {
             for (final DelegatingDriver delegating : builder.getPropagationParametersDrivers().getDrivers()) {
                 if (delegating.isSelected() || !estimatedOnly) {
                     for (final ParameterDriver driver : delegating.getRawDrivers()) {
                         estimated.add(driver);
                     }
                 }
             }
         }
         return estimated;
 
     }
 
     /** Get the measurements parameters supported by this estimator (including measurements and modifiers).
      * @param estimatedOnly if true, only estimated parameters are returned
      * @return measurements parameters supported by this estimator
      */
     public ParameterDriversList getMeasurementsParametersDrivers(final boolean estimatedOnly) {
 
         final ParameterDriversList parameters =  new ParameterDriversList();
         for (final  ObservedMeasurement<?> measurement : measurements) {
             for (final ParameterDriver driver : measurement.getParametersDrivers()) {
                 if (!estimatedOnly || driver.isSelected()) {
                     parameters.add(driver);
                 }
             }
         }
 
         parameters.sort();
 
         return parameters;
 
     }
 
     /**
      * Set convergence threshold.
      * <p>
      * The convergence used for estimation is based on the estimated
      * parameters {@link ParameterDriver#getNormalizedValue() normalized values}.
      * Convergence is considered to have been reached when the difference
      * between previous and current normalized value is less than the
      * convergence threshold for all parameters. The same value is used
      * for all parameters since they are normalized and hence dimensionless.
      * </p>
      * <p>
      * Normalized values are computed as {@code (current - reference)/scale},
      * so convergence is reached when the following condition holds for
      * all estimated parameters:
      * {@code |current[i] - previous[i]| <= threshold * scale[i]}
      * </p>
      * <p>
      * So the convergence threshold specified here can be considered as
      * a multiplication factor applied to scale. Since for all parameters
      * the scale is often small (typically about 1 m for orbital positions
      * for example), then the threshold should not be too small. A value
      * of 10⁻³ is often quite accurate.
      * </p>
      * <p>
      * Calling this method overrides any checker that could have been set
      * beforehand by calling {@link #setConvergenceChecker(ConvergenceChecker)}.
      * Both methods are mutually exclusive.
      * </p>
      *
      * @param parametersConvergenceThreshold convergence threshold on
      * normalized parameters (dimensionless, related to parameters scales)
      * @see #setConvergenceChecker(ConvergenceChecker)
      * @see EvaluationRmsChecker
      */
     public void setParametersConvergenceThreshold(final double parametersConvergenceThreshold) {
         setConvergenceChecker((iteration, previous, current) ->
                               current.getPoint().getLInfDistance(previous.getPoint()) <= parametersConvergenceThreshold);
     }
 
     /** Set a custom convergence checker.
      * <p>
      * Calling this method overrides any checker that could have been set
      * beforehand by calling {@link #setParametersConvergenceThreshold(double)}.
      * Both methods are mutually exclusive.
      * </p>
      * @param convergenceChecker convergence checker to set
      * @see #setParametersConvergenceThreshold(double)
      * @since 10.1
      */
     public void setConvergenceChecker(final ConvergenceChecker<LeastSquaresProblem.Evaluation> convergenceChecker) {
         this.convergenceChecker = convergenceChecker;
     }
 
     /** Estimate the orbital, propagation and measurements parameters.
      * <p>
      * The initial guess for all parameters must have been set before calling this method
      * using {@link #getOrbitalParametersDrivers(boolean)}, {@link #getPropagatorParametersDrivers(boolean)},
      * and {@link #getMeasurementsParametersDrivers(boolean)} and then {@link ParameterDriver#setValue(double)
      * setting the values} of the parameters.
      * </p>
      * <p>
      * For parameters whose reference date has not been set to a non-null date beforehand (i.e.
      * the parameters for which {@link ParameterDriver#getReferenceDate()} returns {@code null},
      * a default reference date will be set automatically at the start of the estimation to the
      * {@link AbstractPropagatorBuilder#getInitialOrbitDate() initial orbit date} of the first
      * propagator builder. For parameters whose reference date has been set to a non-null date,
      * this reference date is untouched.
      * </p>
      * <p>
      * After this method returns, the estimated parameters can be retrieved using
      * {@link #getOrbitalParametersDrivers(boolean)}, {@link #getPropagatorParametersDrivers(boolean)},
      * and {@link #getMeasurementsParametersDrivers(boolean)} and then {@link ParameterDriver#getValue()
      * getting the values} of the parameters.
      * </p>
      * <p>
      * As a convenience, the method also returns a fully configured and ready to use
      * propagator set up with all the estimated values.
      * </p>
      * <p>
      * For even more in-depth information, the {@link #getOptimum()} method provides detailed
      * elements (covariance matrix, estimated parameters standard deviation, weighted Jacobian, RMS,
      * χ², residuals and more).
      * </p>
      * @return propagators configured with estimated orbits as initial states, and all
      * propagators estimated parameters also set
      */
     public Propagator[] estimate() {
 
         // set reference date for all parameters that lack one (including the not estimated parameters)
         for (final ParameterDriver driver : getOrbitalParametersDrivers(false).getDrivers()) {
             if (driver.getReferenceDate() == null) {
                 driver.setReferenceDate(builders[0].getInitialOrbitDate());
             }
         }
         for (final ParameterDriver driver : getPropagatorParametersDrivers(false).getDrivers()) {
             if (driver.getReferenceDate() == null) {
                 driver.setReferenceDate(builders[0].getInitialOrbitDate());
             }
         }
         for (final ParameterDriver driver : getMeasurementsParametersDrivers(false).getDrivers()) {
             if (driver.getReferenceDate() == null) {
                 driver.setReferenceDate(builders[0].getInitialOrbitDate());
             }
         }
 
         // get all estimated parameters
         final ParameterDriversList estimatedOrbitalParameters      = getOrbitalParametersDrivers(true);
         final ParameterDriversList estimatedPropagatorParameters   = getPropagatorParametersDrivers(true);
         final ParameterDriversList estimatedMeasurementsParameters = getMeasurementsParametersDrivers(true);
 
         // create start point
         final double[] start = new double[estimatedOrbitalParameters.getNbParams() +
                                           estimatedPropagatorParameters.getNbParams() +
                                           estimatedMeasurementsParameters.getNbParams()];
         int iStart = 0;
         for (final ParameterDriver driver : estimatedOrbitalParameters.getDrivers()) {
             start[iStart++] = driver.getNormalizedValue();
         }
         for (final ParameterDriver driver : estimatedPropagatorParameters.getDrivers()) {
             start[iStart++] = driver.getNormalizedValue();
         }
         for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
             start[iStart++] = driver.getNormalizedValue();
         }
         lsBuilder.start(start);
 
         // create target (which is an array set to 0, as we compute weighted residuals ourselves)
         int p = 0;
         for (final ObservedMeasurement<?> measurement : measurements) {
             if (measurement.isEnabled()) {
                 p += measurement.getDimension();
             }
         }
         final double[] target = new double[p];
         lsBuilder.target(target);
 
         // set up the model
         final ModelObserver modelObserver = new ModelObserver() {
             /** {@inheritDoc} */
             @Override
             public void modelCalled(final Orbit[] newOrbits,
                                     final Map<ObservedMeasurement<?>, EstimatedMeasurement<?>> newEstimations) {
                 BatchLSEstimator.this.orbits      = newOrbits;
                 BatchLSEstimator.this.estimations = newEstimations;
             }
         };
         final AbstractBatchLSModel model = builders[0].buildLSModel(builders, measurements, estimatedMeasurementsParameters, modelObserver);
 
         lsBuilder.model(model);
 
         // add a validator for orbital parameters
         lsBuilder.parameterValidator(new Validator(estimatedOrbitalParameters,
                                                    estimatedPropagatorParameters,
                                                    estimatedMeasurementsParameters));
 
         lsBuilder.checker(convergenceChecker);
 
         // set up the problem to solve
         final LeastSquaresProblem problem = new TappedLSProblem(lsBuilder.build(),
                                                                 model,
                                                                 estimatedOrbitalParameters,
                                                                 estimatedPropagatorParameters,
                                                                 estimatedMeasurementsParameters);
 
         try {
 
             // solve the problem
             optimum = optimizer.optimize(problem);
 
             // create a new configured propagator with all estimated parameters
             return model.createPropagators(optimum.getPoint());
 
         } catch (MathRuntimeException mrte) {
             throw new OrekitException(mrte);
         }
     }
 
     /** Get the last estimations performed.
      * @return last estimations performed
      */
     public Map<ObservedMeasurement<?>, EstimatedMeasurement<?>> getLastEstimations() {
         return Collections.unmodifiableMap(estimations);
     }
 
     /** Get the optimum found.
      * <p>
      * The {@link Optimum} object contains detailed elements (covariance matrix, estimated
      * parameters standard deviation, weighted Jacobian, RMS, χ², residuals and more).
      * </p>
      * <p>
      * Beware that the returned object is the raw view from the underlying mathematical
      * library. At this raw level, parameters have {@link ParameterDriver#getNormalizedValue()
      * normalized values} whereas the space flight parameters have {@link ParameterDriver#getValue()
      * physical values} with their units. So there are {@link ParameterDriver#getScale() scaling
      * factors} to apply when using these elements.
      * </p>
      * @return optimum found after last call to {@link #estimate()}
      */
     public Optimum getOptimum() {
         return optimum;
     }
 
     /** Get the covariances matrix in space flight dynamics physical units.
      * <p>
      * This method retrieve the {@link
      * org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem.Evaluation#getCovariances(double)
      * covariances} from the [@link {@link #getOptimum() optimum} and applies the scaling factors
      * to it in order to convert it from raw normalized values back to physical values.
      * </p>
      * @param threshold threshold to identify matrix singularity
      * @return covariances matrix in space flight dynamics physical units
      * @since 9.1
      */
     public RealMatrix getPhysicalCovariances(final double threshold) {
         final RealMatrix covariances;
         try {
             // get the normalized matrix
             covariances = optimum.getCovariances(threshold).copy();
         } catch (MathIllegalArgumentException miae) {
             // the problem is singular
             throw new OrekitException(miae);
         }
 
         // retrieve the scaling factors
         final double[] scale = new double[covariances.getRowDimension()];
         int index = 0;
         for (final ParameterDriver driver : getOrbitalParametersDrivers(true).getDrivers()) {
             scale[index++] = driver.getScale();
         }
         for (final ParameterDriver driver : getPropagatorParametersDrivers(true).getDrivers()) {
             scale[index++] = driver.getScale();
         }
         for (final ParameterDriver driver : getMeasurementsParametersDrivers(true).getDrivers()) {
             scale[index++] = driver.getScale();
         }
 
         // unnormalize the matrix, to retrieve physical covariances
         for (int i = 0; i < covariances.getRowDimension(); ++i) {
             for (int j = 0; j < covariances.getColumnDimension(); ++j) {
                 covariances.setEntry(i, j, scale[i] * scale[j] * covariances.getEntry(i, j));
             }
         }
 
         return covariances;
 
     }
 
     /** Get the number of iterations used for last estimation.
      * @return number of iterations used for last estimation
      * @see #setMaxIterations(int)
      */
     public int getIterationsCount() {
         return iterationsCounter.getCount();
     }
 
     /** Get the number of evaluations used for last estimation.
      * @return number of evaluations used for last estimation
      * @see #setMaxEvaluations(int)
      */
     public int getEvaluationsCount() {
         return evaluationsCounter.getCount();
     }
 
     /** Wrapper used to tap the various counters. */
     private class TappedLSProblem implements LeastSquaresProblem {
 
         /** Underlying problem. */
         private final LeastSquaresProblem problem;
 
         /** Multivariate function model. */
         private final AbstractBatchLSModel model;
 
         /** Estimated orbital parameters. */
         private final ParameterDriversList estimatedOrbitalParameters;
 
         /** Estimated propagator parameters. */
         private final ParameterDriversList estimatedPropagatorParameters;
 
         /** Estimated measurements parameters. */
         private final ParameterDriversList estimatedMeasurementsParameters;
 
         /** Simple constructor.
          * @param problem underlying problem
          * @param model multivariate function model
          * @param estimatedOrbitalParameters estimated orbital parameters
          * @param estimatedPropagatorParameters estimated propagator parameters
          * @param estimatedMeasurementsParameters estimated measurements parameters
          */
         TappedLSProblem(final LeastSquaresProblem problem,
                         final AbstractBatchLSModel model,
                         final ParameterDriversList estimatedOrbitalParameters,
                         final ParameterDriversList estimatedPropagatorParameters,
                         final ParameterDriversList estimatedMeasurementsParameters) {
             this.problem                         = problem;
             this.model                           = model;
             this.estimatedOrbitalParameters      = estimatedOrbitalParameters;
             this.estimatedPropagatorParameters   = estimatedPropagatorParameters;
             this.estimatedMeasurementsParameters = estimatedMeasurementsParameters;
         }
 
         /** {@inheritDoc} */
         @Override
         public Incrementor getEvaluationCounter() {
             // tap the evaluations counter
             BatchLSEstimator.this.evaluationsCounter = problem.getEvaluationCounter();
             model.setEvaluationsCounter(BatchLSEstimator.this.evaluationsCounter);
             return BatchLSEstimator.this.evaluationsCounter;
         }
 
         /** {@inheritDoc} */
         @Override
         public Incrementor getIterationCounter() {
             // tap the iterations counter
             BatchLSEstimator.this.iterationsCounter = problem.getIterationCounter();
             model.setIterationsCounter(BatchLSEstimator.this.iterationsCounter);
             return BatchLSEstimator.this.iterationsCounter;
         }
 
         /** {@inheritDoc} */
         @Override
         public ConvergenceChecker<Evaluation> getConvergenceChecker() {
             return problem.getConvergenceChecker();
         }
 
         /** {@inheritDoc} */
         @Override
         public RealVector getStart() {
             return problem.getStart();
         }
 
         /** {@inheritDoc} */
         @Override
         public int getObservationSize() {
             return problem.getObservationSize();
         }
 
         /** {@inheritDoc} */
         @Override
         public int getParameterSize() {
             return problem.getParameterSize();
         }
 
         /** {@inheritDoc} */
         @Override
         public Evaluation evaluate(final RealVector point) {
 
             // perform the evaluation
             final Evaluation evaluation = problem.evaluate(point);
 
             // notify the observer
             if (observer != null) {
                 observer.evaluationPerformed(iterationsCounter.getCount(),
                                              evaluationsCounter.getCount(),
                                              orbits,
                                              estimatedOrbitalParameters,
                                              estimatedPropagatorParameters,
                                              estimatedMeasurementsParameters,
                                              new Provider(),
                                              evaluation);
             }
 
             return evaluation;
 
         }
 
     }
 
     /** Provider for evaluations. */
     private class Provider implements EstimationsProvider {
 
         /** Sorted estimations. */
         private EstimatedMeasurement<?>[] sortedEstimations;
 
         /** {@inheritDoc} */
         @Override
         public int getNumber() {
             return estimations.size();
         }
 
         /** {@inheritDoc} */
         @Override
         public EstimatedMeasurement<?> getEstimatedMeasurement(final int index) {
 
             // safety checks
             if (index < 0 || index >= estimations.size()) {
                 throw new OrekitException(LocalizedCoreFormats.OUT_OF_RANGE_SIMPLE,
                                           index, 0, estimations.size());
             }
 
             if (sortedEstimations == null) {
 
                 // lazy evaluation of the sorted array
                 sortedEstimations = new EstimatedMeasurement<?>[estimations.size()];
                 int i = 0;
                 for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry : estimations.entrySet()) {
                     sortedEstimations[i++] = entry.getValue();
                 }
 
                 // sort the array, primarily chronologically
                 Arrays.sort(sortedEstimations, 0, sortedEstimations.length, Comparator.naturalOrder());
 
             }
 
             return sortedEstimations[index];
 
         }
 
     }
 
     /** Validator for estimated parameters. */
     private static class Validator implements ParameterValidator {
 
         /** Estimated orbital parameters. */
         private final ParameterDriversList estimatedOrbitalParameters;
 
         /** Estimated propagator parameters. */
         private final ParameterDriversList estimatedPropagatorParameters;
 
         /** Estimated measurements parameters. */
         private final ParameterDriversList estimatedMeasurementsParameters;
 
         /** Simple constructor.
          * @param estimatedOrbitalParameters estimated orbital parameters
          * @param estimatedPropagatorParameters estimated propagator parameters
          * @param estimatedMeasurementsParameters estimated measurements parameters
          */
         Validator(final ParameterDriversList estimatedOrbitalParameters,
                   final ParameterDriversList estimatedPropagatorParameters,
                   final ParameterDriversList estimatedMeasurementsParameters) {
             this.estimatedOrbitalParameters      = estimatedOrbitalParameters;
             this.estimatedPropagatorParameters   = estimatedPropagatorParameters;
             this.estimatedMeasurementsParameters = estimatedMeasurementsParameters;
         }
 
         /** {@inheritDoc} */
         @Override
         public RealVector validate(final RealVector params) {
 
             int i = 0;
             for (final ParameterDriver driver : estimatedOrbitalParameters.getDrivers()) {
                 // let the parameter handle min/max clipping
                 driver.setNormalizedValue(params.getEntry(i));
                 params.setEntry(i++, driver.getNormalizedValue());
             }
             for (final ParameterDriver driver : estimatedPropagatorParameters.getDrivers()) {
                 // let the parameter handle min/max clipping
                 driver.setNormalizedValue(params.getEntry(i));
                 params.setEntry(i++, driver.getNormalizedValue());
             }
             for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
                 // let the parameter handle min/max clipping
                 driver.setNormalizedValue(params.getEntry(i));
                 params.setEntry(i++, driver.getNormalizedValue());
             }
 
             return params;
         }
     }
 
 }