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;
18
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;
25
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;
59
60
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 {
73
74 /** Builders for propagator. */
75 private final PropagatorBuilder[] builders;
76
77 /** Measurements. */
78 private final List<ObservedMeasurement<?>> measurements;
79
80 /** Solver for least squares problem. */
81 private final LeastSquaresOptimizer optimizer;
82
83 /** Convergence checker. */
84 private ConvergenceChecker<LeastSquaresProblem.Evaluation> convergenceChecker;
85
86 /** Builder for the least squares problem. */
87 private final LeastSquaresBuilder lsBuilder;
88
89 /** Observer for iterations. */
90 private BatchLSObserver observer;
91
92 /** Last estimations. */
93 private Map<ObservedMeasurement<?>, EstimatedMeasurement<?>> estimations;
94
95 /** Last orbits. */
96 private Orbit[] orbits;
97
98 /** Optimum found. */
99 private Optimum optimum;
100
101 /** Counter for the evaluations. */
102 private Incrementor evaluationsCounter;
103
104 /** Counter for the iterations. */
105 private Incrementor iterationsCounter;
106
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) {
128
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];
136
137 setParametersConvergenceThreshold(Double.NaN);
138
139 // our model computes value and Jacobian in one call,
140 // so we don't use the lazy evaluation feature
141 lsBuilder.lazyEvaluation(false);
142
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);
147
148 }
149
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 }
156
157 /** Add a measurement.
158 * @param measurement measurement to add
159 */
160 public void addMeasurement(final ObservedMeasurement<?> measurement) {
161 measurements.add(measurement);
162 }
163
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 }
176
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 }
200
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) {
214
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;
232
233 }
234
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) {
240
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;
252
253 }
254
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) {
260
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 }
269
270 parameters.sort();
271
272 return parameters;
273
274 }
275
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 }
314
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 }
328
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() {
363
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 }
380
381 // get all estimated parameters
382 final ParameterDriversList estimatedOrbitalParameters = getOrbitalParametersDrivers(true);
383 final ParameterDriversList estimatedPropagatorParameters = getPropagatorParametersDrivers(true);
384 final ParameterDriversList estimatedMeasurementsParameters = getMeasurementsParametersDrivers(true);
385
386 // create start point
387 final double[] start = new double[estimatedOrbitalParameters.getNbValuesToEstimate() +
388 estimatedPropagatorParameters.getNbValuesToEstimate() +
389 estimatedMeasurementsParameters.getNbValuesToEstimate()];
390
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);
417
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);
427
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);
439
440 lsBuilder.model(model);
441
442 // add a validator for orbital parameters
443 lsBuilder.parameterValidator(new Validator(estimatedOrbitalParameters,
444 estimatedPropagatorParameters,
445 estimatedMeasurementsParameters));
446
447 lsBuilder.checker(convergenceChecker);
448
449 // set up the problem to solve
450 final LeastSquaresProblem problem = new TappedLSProblem(lsBuilder.build(),
451 model,
452 estimatedOrbitalParameters,
453 estimatedPropagatorParameters,
454 estimatedMeasurementsParameters);
455
456 try {
457
458 // solve the problem
459 optimum = optimizer.optimize(problem);
460
461 // create a new configured propagator with all estimated parameters
462 return model.createPropagators(optimum.getPoint());
463
464 } catch (MathRuntimeException mrte) {
465 throw new OrekitException(mrte);
466 }
467 }
468
469 /** Get the last estimations performed.
470 * @return last estimations performed
471 */
472 public Map<ObservedMeasurement<?>, EstimatedMeasurement<?>> getLastEstimations() {
473 return Collections.unmodifiableMap(estimations);
474 }
475
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 }
493
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 }
514
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 }
533
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 }
540
541 return covariances;
542
543 }
544
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 }
552
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 }
560
561 /** Wrapper used to tap the various counters. */
562 private class TappedLSProblem implements LeastSquaresProblem {
563
564 /** Underlying problem. */
565 private final LeastSquaresProblem problem;
566
567 /** Multivariate function model. */
568 private final AbstractBatchLSModel model;
569
570 /** Estimated orbital parameters. */
571 private final ParameterDriversList estimatedOrbitalParameters;
572
573 /** Estimated propagator parameters. */
574 private final ParameterDriversList estimatedPropagatorParameters;
575
576 /** Estimated measurements parameters. */
577 private final ParameterDriversList estimatedMeasurementsParameters;
578
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 }
597
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 }
606
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 }
615
616 /** {@inheritDoc} */
617 @Override
618 public ConvergenceChecker<Evaluation> getConvergenceChecker() {
619 return problem.getConvergenceChecker();
620 }
621
622 /** {@inheritDoc} */
623 @Override
624 public RealVector getStart() {
625 return problem.getStart();
626 }
627
628 /** {@inheritDoc} */
629 @Override
630 public int getObservationSize() {
631 return problem.getObservationSize();
632 }
633
634 /** {@inheritDoc} */
635 @Override
636 public int getParameterSize() {
637 return problem.getParameterSize();
638 }
639
640 /** {@inheritDoc} */
641 @Override
642 public Evaluation evaluate(final RealVector point) {
643
644 // perform the evaluation
645 final Evaluation evaluation = problem.evaluate(point);
646
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 }
658
659 return evaluation;
660
661 }
662
663 }
664
665 /** Provider for evaluations. */
666 private class Provider implements EstimationsProvider {
667
668 /** Sorted estimations. */
669 private EstimatedMeasurement<?>[] sortedEstimations;
670
671 /** {@inheritDoc} */
672 @Override
673 public int getNumber() {
674 return estimations.size();
675 }
676
677 /** {@inheritDoc} */
678 @Override
679 public EstimatedMeasurement<?> getEstimatedMeasurement(final int index) {
680
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 }
686
687 if (sortedEstimations == null) {
688
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 }
695
696 // sort the array, primarily chronologically
697 Arrays.sort(sortedEstimations, 0, sortedEstimations.length, Comparator.naturalOrder());
698
699 }
700
701 return sortedEstimations[index];
702
703 }
704
705 }
706
707 /** Validator for estimated parameters. */
708 private static class Validator implements ParameterValidator {
709
710 /** Estimated orbital parameters. */
711 private final ParameterDriversList estimatedOrbitalParameters;
712
713 /** Estimated propagator parameters. */
714 private final ParameterDriversList estimatedPropagatorParameters;
715
716 /** Estimated measurements parameters. */
717 private final ParameterDriversList estimatedMeasurementsParameters;
718
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 }
731
732 /** {@inheritDoc} */
733 @Override
734 public RealVector validate(final RealVector params) {
735
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));
742
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()));
749
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 }
758
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));
765
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()));
772
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));
787
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()));
794
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 }
807
808 }