BatchLSEstimator.java

  1. /* Copyright 2002-2017 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.estimation.leastsquares;

  18. import java.util.ArrayList;
  19. import java.util.Arrays;
  20. import java.util.Collections;
  21. import java.util.List;
  22. import java.util.Map;

  23. import org.hipparchus.exception.LocalizedCoreFormats;
  24. import org.hipparchus.exception.MathRuntimeException;
  25. import org.hipparchus.linear.RealVector;
  26. import org.hipparchus.optim.ConvergenceChecker;
  27. import org.hipparchus.optim.nonlinear.vector.leastsquares.EvaluationRmsChecker;
  28. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresBuilder;
  29. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer;
  30. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer.Optimum;
  31. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem;
  32. import org.hipparchus.optim.nonlinear.vector.leastsquares.ParameterValidator;
  33. import org.hipparchus.util.Incrementor;
  34. import org.orekit.errors.OrekitException;
  35. import org.orekit.errors.OrekitExceptionWrapper;
  36. import org.orekit.estimation.measurements.EstimatedMeasurement;
  37. import org.orekit.estimation.measurements.EstimationsProvider;
  38. import org.orekit.estimation.measurements.ObservedMeasurement;
  39. import org.orekit.orbits.Orbit;
  40. import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
  41. import org.orekit.propagation.conversion.PropagatorBuilder;
  42. import org.orekit.propagation.numerical.NumericalPropagator;
  43. import org.orekit.time.ChronologicalComparator;
  44. import org.orekit.utils.ParameterDriver;
  45. import org.orekit.utils.ParameterDriversList;
  46. import org.orekit.utils.ParameterDriversList.DelegatingDriver;


  47. /** Least squares estimator for orbit determination.
  48.  * @author Luc Maisonobe
  49.  * @since 8.0
  50.  */
  51. public class BatchLSEstimator {

  52.     /** Builders for propagator. */
  53.     private final NumericalPropagatorBuilder[] builders;

  54.     /** Measurements. */
  55.     private final List<ObservedMeasurement<?>> measurements;

  56.     /** Solver for least squares problem. */
  57.     private final LeastSquaresOptimizer optimizer;

  58.     /** Convergence threshold on normalized parameters. */
  59.     private double parametersConvergenceThreshold;

  60.     /** Builder for the least squares problem. */
  61.     private final LeastSquaresBuilder lsBuilder;

  62.     /** Observer for iterations. */
  63.     private BatchLSObserver observer;

  64.     /** Last estimations. */
  65.     private Map<ObservedMeasurement<?>, EstimatedMeasurement<?>> estimations;

  66.     /** Last orbits. */
  67.     private Orbit[] orbits;

  68.     /** Optimum found. */
  69.     private Optimum optimum;

  70.     /** Counter for the evaluations. */
  71.     private Incrementor evaluationsCounter;

  72.     /** Counter for the iterations. */
  73.     private Incrementor iterationsCounter;

  74.     /** Simple constructor.
  75.      * <p>
  76.      * If multiple {@link PropagatorBuilder propagator builders} are set up,
  77.      * the orbits of several spacecrafts will be used simultaneously.
  78.      * This is useful if the propagators share some model or measurements
  79.      * parameters (typically pole motion, prime meridian correction or
  80.      * ground stations positions).
  81.      * </p>
  82.      * <p>
  83.      * Setting up multiple {@link PropagatorBuilder propagator builders} is
  84.      * also useful when inter-satellite measurements are used, even if only one
  85.      * of the orbit is estimated and the other ones are fixed. This is typically
  86.      * used when very high accuracy GNSS measurements are needed and the
  87.      * navigation bulletins are not considered accurate enough and the navigation
  88.      * constellation must be propagated numerically.
  89.      * </p>
  90.      * @param optimizer solver for least squares problem
  91.      * @param propagatorBuilder builders to use for propagation
  92.      * @exception OrekitException if some propagator parameter cannot be retrieved
  93.      */
  94.     public BatchLSEstimator(final LeastSquaresOptimizer optimizer,
  95.                             final NumericalPropagatorBuilder... propagatorBuilder)
  96.         throws OrekitException {

  97.         this.builders                       = propagatorBuilder;
  98.         this.measurements                   = new ArrayList<ObservedMeasurement<?>>();
  99.         this.optimizer                      = optimizer;
  100.         this.parametersConvergenceThreshold = Double.NaN;
  101.         this.lsBuilder                      = new LeastSquaresBuilder();
  102.         this.observer                       = null;
  103.         this.estimations                    = null;
  104.         this.orbits                         = new Orbit[builders.length];

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

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

  112.     }

  113.     /** Set an observer for iterations.
  114.      * @param observer observer to be notified at the end of each iteration
  115.      */
  116.     public void setObserver(final BatchLSObserver observer) {
  117.         this.observer = observer;
  118.     }

  119.     /** Add a measurement.
  120.      * @param measurement measurement to add
  121.      * @exception OrekitException if the measurement has a parameter
  122.      * that is already used
  123.      */
  124.     public void addMeasurement(final ObservedMeasurement<?> measurement)
  125.         throws OrekitException {
  126.         measurements.add(measurement);
  127.     }

  128.     /** Set the maximum number of iterations.
  129.      * <p>
  130.      * The iterations correspond to the top level iterations of
  131.      * the {@link LeastSquaresOptimizer least squares optimizer}.
  132.      * </p>
  133.      * @param maxIterations maxIterations maximum number of iterations
  134.      * @see #setMaxEvaluations(int)
  135.      * @see #getIterationsCount()
  136.      */
  137.     public void setMaxIterations(final int maxIterations) {
  138.         lsBuilder.maxIterations(maxIterations);
  139.     }

  140.     /** Set the maximum number of model evaluations.
  141.      * <p>
  142.      * The evaluations correspond to the orbit propagations and
  143.      * measurements estimations performed with a set of estimated
  144.      * parameters.
  145.      * </p>
  146.      * <p>
  147.      * For {@link org.hipparchus.optim.nonlinear.vector.leastsquares.GaussNewtonOptimizer
  148.      * Gauss-Newton optimizer} there is one evaluation at each iteration,
  149.      * so the maximum numbers may be set to the same value. For {@link
  150.      * org.hipparchus.optim.nonlinear.vector.leastsquares.LevenbergMarquardtOptimizer
  151.      * Levenberg-Marquardt optimizer}, there can be several evaluations at
  152.      * some iterations (typically for the first couple of iterations), so the
  153.      * maximum number of evaluations may be set to a higher value than the
  154.      * maximum number of iterations.
  155.      * </p>
  156.      * @param maxEvaluations maximum number of model evaluations
  157.      * @see #setMaxIterations(int)
  158.      * @see #getEvaluationsCount()
  159.      */
  160.     public void setMaxEvaluations(final int maxEvaluations) {
  161.         lsBuilder.maxEvaluations(maxEvaluations);
  162.     }

  163.     /** Get the orbital parameters supported by this estimator.
  164.      * <p>
  165.      * If there are more than one propagator builder, then the names
  166.      * of the drivers have an index marker in square brackets appended
  167.      * to them in order to distinguish the various orbits. So for example
  168.      * with one builder generating Keplerian orbits the names would be
  169.      * simply "a", "e", "i"... but if there are several builders the
  170.      * names would be "a[0]", "e[0]", "i[0]"..."a[1]", "e[1]", "i[1]"...
  171.      * </p>
  172.      * @param estimatedOnly if true, only estimated parameters are returned
  173.      * @return orbital parameters supported by this estimator
  174.      * @exception OrekitException if different parameters have the same name
  175.      */
  176.     public ParameterDriversList getOrbitalParametersDrivers(final boolean estimatedOnly)
  177.         throws OrekitException {

  178.         final ParameterDriversList estimated = new ParameterDriversList();
  179.         for (int i = 0; i < builders.length; ++i) {
  180.             final String suffix = builders.length > 1 ? "[" + i + "]" : null;
  181.             for (final DelegatingDriver delegating : builders[i].getOrbitalParametersDrivers().getDrivers()) {
  182.                 if (delegating.isSelected() || !estimatedOnly) {
  183.                     for (final ParameterDriver driver : delegating.getRawDrivers()) {
  184.                         if (suffix != null && !driver.getName().endsWith(suffix)) {
  185.                             // we add suffix only conditionally because the method may already have been called
  186.                             // and suffixes may have already been appended
  187.                             driver.setName(driver.getName() + suffix);
  188.                         }
  189.                         estimated.add(driver);
  190.                     }
  191.                 }
  192.             }
  193.         }
  194.         return estimated;

  195.     }

  196.     /** Get the propagator parameters supported by this estimator.
  197.      * @param estimatedOnly if true, only estimated parameters are returned
  198.      * @return propagator parameters supported by this estimator
  199.      * @exception OrekitException if different parameters have the same name
  200.      */
  201.     public ParameterDriversList getPropagatorParametersDrivers(final boolean estimatedOnly)
  202.         throws OrekitException {

  203.         final ParameterDriversList estimated = new ParameterDriversList();
  204.         for (PropagatorBuilder builder : builders) {
  205.             for (final DelegatingDriver delegating : builder.getPropagationParametersDrivers().getDrivers()) {
  206.                 if (delegating.isSelected() || !estimatedOnly) {
  207.                     for (final ParameterDriver driver : delegating.getRawDrivers()) {
  208.                         estimated.add(driver);
  209.                     }
  210.                 }
  211.             }
  212.         }
  213.         return estimated;

  214.     }

  215.     /** Get the measurements parameters supported by this estimator (including measurements and modifiers).
  216.      * @param estimatedOnly if true, only estimated parameters are returned
  217.      * @return measurements parameters supported by this estimator
  218.      * @exception OrekitException if different parameters have the same name
  219.      */
  220.     public ParameterDriversList getMeasurementsParametersDrivers(final boolean estimatedOnly)
  221.         throws OrekitException {

  222.         final ParameterDriversList parameters =  new ParameterDriversList();
  223.         for (final  ObservedMeasurement<?> measurement : measurements) {
  224.             for (final ParameterDriver driver : measurement.getParametersDrivers()) {
  225.                 if ((!estimatedOnly) || driver.isSelected()) {
  226.                     parameters.add(driver);
  227.                 }
  228.             }
  229.         }

  230.         parameters.sort();

  231.         return parameters;

  232.     }

  233.     /**
  234.      * Set convergence threshold.
  235.      * <p>
  236.      * The convergence used for estimation is based on the estimated
  237.      * parameters {@link ParameterDriver#getNormalizedValue() normalized values}.
  238.      * Convergence is considered to have been reached when the difference
  239.      * between previous and current normalized value is less than the
  240.      * convergence threshold for all parameters. The same value is used
  241.      * for all parameters since they are normalized and hence dimensionless.
  242.      * </p>
  243.      * <p>
  244.      * Normalized values are computed as {@code (current - reference)/scale},
  245.      * so convergence is reached when the following condition holds for
  246.      * all estimated parameters:
  247.      * {@code |current[i] - previous[i]| <= threshold * scale[i]}
  248.      * </p>
  249.      * <p>
  250.      * So the convergence threshold specified here can be considered as
  251.      * a multiplication factor applied to scale. Since for all parameters
  252.      * the scale is often small (typically about 1 m for orbital positions
  253.      * for example), then the threshold should not be too small. A value
  254.      * of 10⁻³ is often quite accurate.
  255.      *
  256.      * @param parametersConvergenceThreshold convergence threshold on
  257.      * normalized parameters (dimensionless, related to parameters scales)
  258.      * @see EvaluationRmsChecker
  259.      */
  260.     public void setParametersConvergenceThreshold(final double parametersConvergenceThreshold) {
  261.         this.parametersConvergenceThreshold = parametersConvergenceThreshold;
  262.     }

  263.     /** Estimate the orbital, propagation and measurements parameters.
  264.      * <p>
  265.      * The initial guess for all parameters must have been set before calling this method
  266.      * using {@link #getOrbitalParametersDrivers(boolean)}, {@link #getPropagatorParametersDrivers(boolean)},
  267.      * and {@link #getMeasurementsParametersDrivers(boolean)} and then {@link ParameterDriver#setValue(double)
  268.      * setting the values} of the parameters.
  269.      * </p>
  270.      * <p>
  271.      * For parameters whose reference date has not been set to a non-null date beforehand (i.e.
  272.      * the parameters for which {@link ParameterDriver#getReferenceDate()} returns {@code null},
  273.      * a default reference date will be set automatically at the start of the estimation to the
  274.      * {@link NumericalPropagatorBuilder#getInitialOrbitDate() initial orbit date} of the first
  275.      * propagator builder. For parameters whose reference date has been set to a non-null date,
  276.      * this reference date is untouched.
  277.      * </p>
  278.      * <p>
  279.      * After this method returns, the estimated parameters can be retrieved using
  280.      * {@link #getOrbitalParametersDrivers(boolean)}, {@link #getPropagatorParametersDrivers(boolean)},
  281.      * and {@link #getMeasurementsParametersDrivers(boolean)} and then {@link ParameterDriver#getValue()
  282.      * getting the values} of the parameters.
  283.      * </p>
  284.      * <p>
  285.      * As a convenience, the method also returns a fully configured and ready to use
  286.      * propagator set up with all the estimated values.
  287.      * </p>
  288.      * <p>
  289.      * For even more in-depth information, the {@link #getOptimum()} method provides detailed
  290.      * elements (covariance matrix, estimated parameters standard deviation, weighted Jacobian, RMS,
  291.      * χ², residuals and more).
  292.      * </p>
  293.      * @return propagators configured with estimated orbits as initial states, and all
  294.      * propagators estimated parameters also set
  295.      * @exception OrekitException if there is a conflict in parameters names
  296.      * or if orbit cannot be determined
  297.      */
  298.     public NumericalPropagator[] estimate() throws OrekitException {

  299.         // set reference date for all parameters that lack one (including the not estimated parameters)
  300.         for (final ParameterDriver driver : getOrbitalParametersDrivers(false).getDrivers()) {
  301.             if (driver.getReferenceDate() == null) {
  302.                 driver.setReferenceDate(builders[0].getInitialOrbitDate());
  303.             }
  304.         }
  305.         for (final ParameterDriver driver : getPropagatorParametersDrivers(false).getDrivers()) {
  306.             if (driver.getReferenceDate() == null) {
  307.                 driver.setReferenceDate(builders[0].getInitialOrbitDate());
  308.             }
  309.         }
  310.         for (final ParameterDriver driver : getMeasurementsParametersDrivers(false).getDrivers()) {
  311.             if (driver.getReferenceDate() == null) {
  312.                 driver.setReferenceDate(builders[0].getInitialOrbitDate());
  313.             }
  314.         }

  315.         // get all estimated parameters
  316.         final ParameterDriversList estimatedOrbitalParameters      = getOrbitalParametersDrivers(true);
  317.         final ParameterDriversList estimatedPropagatorParameters   = getPropagatorParametersDrivers(true);
  318.         final ParameterDriversList estimatedMeasurementsParameters = getMeasurementsParametersDrivers(true);

  319.         // create start point
  320.         final double[] start = new double[estimatedOrbitalParameters.getNbParams() +
  321.                                           estimatedPropagatorParameters.getNbParams() +
  322.                                           estimatedMeasurementsParameters.getNbParams()];
  323.         int iStart = 0;
  324.         for (final ParameterDriver driver : estimatedOrbitalParameters.getDrivers()) {
  325.             start[iStart++] = driver.getNormalizedValue();
  326.         }
  327.         for (final ParameterDriver driver : estimatedPropagatorParameters.getDrivers()) {
  328.             start[iStart++] = driver.getNormalizedValue();
  329.         }
  330.         for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
  331.             start[iStart++] = driver.getNormalizedValue();
  332.         }
  333.         lsBuilder.start(start);

  334.         // create target (which is an array set to 0, as we compute weighted residuals ourselves)
  335.         int p = 0;
  336.         for (final ObservedMeasurement<?> measurement : measurements) {
  337.             if (measurement.isEnabled()) {
  338.                 p += measurement.getDimension();
  339.             }
  340.         }
  341.         final double[] target = new double[p];
  342.         lsBuilder.target(target);

  343.         // set up the model
  344.         final ModelObserver modelObserver = new ModelObserver() {
  345.             /** {@inheritDoc} */
  346.             @Override
  347.             public void modelCalled(final Orbit[] newOrbits,
  348.                                     final Map<ObservedMeasurement<?>, EstimatedMeasurement<?>> newEstimations) {
  349.                 BatchLSEstimator.this.orbits      = newOrbits;
  350.                 BatchLSEstimator.this.estimations = newEstimations;
  351.             }
  352.         };
  353.         final Model model = new Model(builders, measurements, estimatedMeasurementsParameters,
  354.                                       modelObserver);
  355.         lsBuilder.model(model);

  356.         // add a validator for orbital parameters
  357.         lsBuilder.parameterValidator(new Validator(estimatedOrbitalParameters,
  358.                                                    estimatedPropagatorParameters,
  359.                                                    estimatedMeasurementsParameters));

  360.         lsBuilder.checker(new ConvergenceChecker<LeastSquaresProblem.Evaluation>() {
  361.             /** {@inheritDoc} */
  362.             @Override
  363.             public boolean converged(final int iteration,
  364.                                      final LeastSquaresProblem.Evaluation previous,
  365.                                      final LeastSquaresProblem.Evaluation current) {
  366.                 final double lInf = current.getPoint().getLInfDistance(previous.getPoint());
  367.                 return lInf <= parametersConvergenceThreshold;
  368.             }
  369.         });

  370.         // set up the problem to solve
  371.         final LeastSquaresProblem problem = new TappedLSProblem(lsBuilder.build(),
  372.                                                                 model,
  373.                                                                 estimatedOrbitalParameters,
  374.                                                                 estimatedPropagatorParameters,
  375.                                                                 estimatedMeasurementsParameters);

  376.         try {

  377.             // solve the problem
  378.             optimum = optimizer.optimize(problem);

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

  381.         } catch (MathRuntimeException mrte) {
  382.             throw new OrekitException(mrte);
  383.         } catch (OrekitExceptionWrapper oew) {
  384.             throw oew.getException();
  385.         }

  386.     }

  387.     /** Get the last estimations performed.
  388.      * @return last estimations performed
  389.      */
  390.     public Map<ObservedMeasurement<?>, EstimatedMeasurement<?>> getLastEstimations() {
  391.         return Collections.unmodifiableMap(estimations);
  392.     }

  393.     /** Get the optimum found.
  394.      * <p>
  395.      * The {@link Optimum} object contains detailed elements (covariance matrix, estimated
  396.      * parameters standard deviation, weighted Jacobian, RMS, χ², residuals and more).
  397.      * </p>
  398.      * @return optimum found after last call to {@link #estimate()}
  399.      */
  400.     public Optimum getOptimum() {
  401.         return optimum;
  402.     }

  403.     /** Get the number of iterations used for last estimation.
  404.      * @return number of iterations used for last estimation
  405.      * @see #setMaxIterations(int)
  406.      */
  407.     public int getIterationsCount() {
  408.         return iterationsCounter.getCount();
  409.     }

  410.     /** Get the number of evaluations used for last estimation.
  411.      * @return number of evaluations used for last estimation
  412.      * @see #setMaxEvaluations(int)
  413.      */
  414.     public int getEvaluationsCount() {
  415.         return evaluationsCounter.getCount();
  416.     }

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

  419.         /** Underlying problem. */
  420.         private final LeastSquaresProblem problem;

  421.         /** Multivariate function model. */
  422.         private final Model model;

  423.         /** Estimated orbital parameters. */
  424.         private final ParameterDriversList estimatedOrbitalParameters;

  425.         /** Estimated propagator parameters. */
  426.         private final ParameterDriversList estimatedPropagatorParameters;

  427.         /** Estimated measurements parameters. */
  428.         private final ParameterDriversList estimatedMeasurementsParameters;

  429.         /** Simple constructor.
  430.          * @param problem underlying problem
  431.          * @param model multivariate function model
  432.          * @param estimatedOrbitalParameters estimated orbital parameters
  433.          * @param estimatedPropagatorParameters estimated propagator parameters
  434.          * @param estimatedMeasurementsParameters estimated measurements parameters
  435.          */
  436.         TappedLSProblem(final LeastSquaresProblem problem,
  437.                         final Model model,
  438.                         final ParameterDriversList estimatedOrbitalParameters,
  439.                         final ParameterDriversList estimatedPropagatorParameters,
  440.                         final ParameterDriversList estimatedMeasurementsParameters) {
  441.             this.problem                         = problem;
  442.             this.model                           = model;
  443.             this.estimatedOrbitalParameters      = estimatedOrbitalParameters;
  444.             this.estimatedPropagatorParameters   = estimatedPropagatorParameters;
  445.             this.estimatedMeasurementsParameters = estimatedMeasurementsParameters;
  446.         }

  447.         /** {@inheritDoc} */
  448.         @Override
  449.         public Incrementor getEvaluationCounter() {
  450.             // tap the evaluations counter
  451.             BatchLSEstimator.this.evaluationsCounter = problem.getEvaluationCounter();
  452.             model.setEvaluationsCounter(BatchLSEstimator.this.evaluationsCounter);
  453.             return BatchLSEstimator.this.evaluationsCounter;
  454.         }

  455.         /** {@inheritDoc} */
  456.         @Override
  457.         public Incrementor getIterationCounter() {
  458.             // tap the iterations counter
  459.             BatchLSEstimator.this.iterationsCounter = problem.getIterationCounter();
  460.             model.setIterationsCounter(BatchLSEstimator.this.iterationsCounter);
  461.             return BatchLSEstimator.this.iterationsCounter;
  462.         }

  463.         /** {@inheritDoc} */
  464.         @Override
  465.         public ConvergenceChecker<Evaluation> getConvergenceChecker() {
  466.             return problem.getConvergenceChecker();
  467.         }

  468.         /** {@inheritDoc} */
  469.         @Override
  470.         public RealVector getStart() {
  471.             return problem.getStart();
  472.         }

  473.         /** {@inheritDoc} */
  474.         @Override
  475.         public int getObservationSize() {
  476.             return problem.getObservationSize();
  477.         }

  478.         /** {@inheritDoc} */
  479.         @Override
  480.         public int getParameterSize() {
  481.             return problem.getParameterSize();
  482.         }

  483.         /** {@inheritDoc} */
  484.         @Override
  485.         public Evaluation evaluate(final RealVector point) {

  486.             // perform the evaluation
  487.             final Evaluation evaluation = problem.evaluate(point);

  488.             // notify the observer
  489.             if (observer != null) {
  490.                 try {
  491.                     observer.evaluationPerformed(iterationsCounter.getCount(),
  492.                                                  evaluationsCounter.getCount(),
  493.                                                  orbits,
  494.                                                  estimatedOrbitalParameters,
  495.                                                  estimatedPropagatorParameters,
  496.                                                  estimatedMeasurementsParameters,
  497.                                                  new Provider(),
  498.                                                  evaluation);
  499.                 } catch (OrekitException oe) {
  500.                     throw new OrekitExceptionWrapper(oe);
  501.                 }
  502.             }

  503.             return evaluation;

  504.         }

  505.     }

  506.     /** Provider for evaluations. */
  507.     private class Provider implements EstimationsProvider {

  508.         /** Sorted estimations. */
  509.         private EstimatedMeasurement<?>[] sortedEstimations;

  510.         /** {@inheritDoc} */
  511.         @Override
  512.         public int getNumber() {
  513.             return estimations.size();
  514.         }

  515.         /** {@inheritDoc} */
  516.         @Override
  517.         public EstimatedMeasurement<?> getEstimatedMeasurement(final int index)
  518.             throws OrekitException {

  519.             // safety checks
  520.             if (index < 0 || index >= estimations.size()) {
  521.                 throw new OrekitException(LocalizedCoreFormats.OUT_OF_RANGE_SIMPLE,
  522.                                           index, 0, estimations.size());
  523.             }

  524.             if (sortedEstimations == null) {

  525.                 // lazy evaluation of the sorted array
  526.                 sortedEstimations = new EstimatedMeasurement<?>[estimations.size()];
  527.                 int i = 0;
  528.                 for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry : estimations.entrySet()) {
  529.                     sortedEstimations[i++] = entry.getValue();
  530.                 }

  531.                 // sort the array chronologically
  532.                 Arrays.sort(sortedEstimations, 0, sortedEstimations.length,
  533.                             new ChronologicalComparator());

  534.             }

  535.             return sortedEstimations[index];

  536.         }

  537.     }

  538.     /** Validator for estimated parameters. */
  539.     private static class Validator implements ParameterValidator {

  540.         /** Estimated orbital parameters. */
  541.         private final ParameterDriversList estimatedOrbitalParameters;

  542.         /** Estimated propagator parameters. */
  543.         private final ParameterDriversList estimatedPropagatorParameters;

  544.         /** Estimated measurements parameters. */
  545.         private final ParameterDriversList estimatedMeasurementsParameters;

  546.         /** Simple constructor.
  547.          * @param estimatedOrbitalParameters estimated orbital parameters
  548.          * @param estimatedPropagatorParameters estimated propagator parameters
  549.          * @param estimatedMeasurementsParameters estimated measurements parameters
  550.          */
  551.         Validator(final ParameterDriversList estimatedOrbitalParameters,
  552.                   final ParameterDriversList estimatedPropagatorParameters,
  553.                   final ParameterDriversList estimatedMeasurementsParameters) {
  554.             this.estimatedOrbitalParameters      = estimatedOrbitalParameters;
  555.             this.estimatedPropagatorParameters   = estimatedPropagatorParameters;
  556.             this.estimatedMeasurementsParameters = estimatedMeasurementsParameters;
  557.         }

  558.         /** {@inheritDoc} */
  559.         @Override
  560.         public RealVector validate(final RealVector params)
  561.             throws OrekitExceptionWrapper {

  562.             try {
  563.                 int i = 0;
  564.                 for (final ParameterDriver driver : estimatedOrbitalParameters.getDrivers()) {
  565.                     // let the parameter handle min/max clipping
  566.                     driver.setNormalizedValue(params.getEntry(i));
  567.                     params.setEntry(i++, driver.getNormalizedValue());
  568.                 }
  569.                 for (final ParameterDriver driver : estimatedPropagatorParameters.getDrivers()) {
  570.                     // let the parameter handle min/max clipping
  571.                     driver.setNormalizedValue(params.getEntry(i));
  572.                     params.setEntry(i++, driver.getNormalizedValue());
  573.                 }
  574.                 for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
  575.                     // let the parameter handle min/max clipping
  576.                     driver.setNormalizedValue(params.getEntry(i));
  577.                     params.setEntry(i++, driver.getNormalizedValue());
  578.                 }

  579.                 return params;
  580.             } catch (OrekitException oe) {
  581.                 throw new OrekitExceptionWrapper(oe);
  582.             }
  583.         }
  584.     }

  585. }