1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.leastsquares;
18
19 import org.hipparchus.linear.Array2DRowRealMatrix;
20 import org.hipparchus.linear.ArrayRealVector;
21 import org.hipparchus.linear.MatrixUtils;
22 import org.hipparchus.linear.RealMatrix;
23 import org.hipparchus.linear.RealVector;
24 import org.hipparchus.optim.nonlinear.vector.leastsquares.MultivariateJacobianFunction;
25 import org.hipparchus.util.FastMath;
26 import org.hipparchus.util.Incrementor;
27 import org.hipparchus.util.Pair;
28 import org.orekit.estimation.measurements.EstimatedMeasurement;
29 import org.orekit.estimation.measurements.EstimatedMeasurementBase;
30 import org.orekit.estimation.measurements.ObservedMeasurement;
31 import org.orekit.orbits.Orbit;
32 import org.orekit.propagation.MatricesHarvester;
33 import org.orekit.propagation.Propagator;
34 import org.orekit.propagation.PropagatorsParallelizer;
35 import org.orekit.propagation.SpacecraftState;
36 import org.orekit.propagation.conversion.PropagatorBuilder;
37 import org.orekit.propagation.sampling.MultiSatStepHandler;
38 import org.orekit.time.AbsoluteDate;
39 import org.orekit.time.ChronologicalComparator;
40 import org.orekit.utils.ParameterDriver;
41 import org.orekit.utils.ParameterDriversList;
42 import org.orekit.utils.ParameterDriversList.DelegatingDriver;
43 import org.orekit.utils.TimeSpanMap;
44 import org.orekit.utils.TimeSpanMap.Span;
45
46 import java.util.ArrayList;
47 import java.util.Arrays;
48 import java.util.Collections;
49 import java.util.HashMap;
50 import java.util.IdentityHashMap;
51 import java.util.List;
52 import java.util.Map;
53
54
55
56
57
58
59
60
61
62
63 public abstract class AbstractBatchLSModel implements MultivariateJacobianFunction {
64
65
66 private final PropagatorBuilder[] builders;
67
68
69
70
71
72 private final ParameterDriversList[] estimatedOrbitalParameters;
73
74
75 private final ParameterDriversList[] estimatedPropagationParameters;
76
77
78 private final ParameterDriversList estimatedMeasurementsParameters;
79
80
81 private final List<ObservedMeasurement<?>> measurements;
82
83
84 private final int[] orbitsStartColumns;
85
86
87 private final int[] orbitsEndColumns;
88
89
90
91
92 private final int[] orbitsJacobianColumns;
93
94
95 private final Map<String, Integer> propagationParameterColumns;
96
97
98 private final Map<String, Integer> measurementParameterColumns;
99
100
101 private final Map<ObservedMeasurement<?>, EstimatedMeasurement<?>> evaluations;
102
103
104 private final ModelObserver observer;
105
106
107 private Incrementor evaluationsCounter;
108
109
110 private Incrementor iterationsCounter;
111
112
113 private AbsoluteDate firstDate;
114
115
116 private AbsoluteDate lastDate;
117
118
119 private final boolean forwardPropagation;
120
121
122 private final RealVector value;
123
124
125
126
127 private final MatricesHarvester[] harvesters;
128
129
130
131
132
133
134
135
136
137 private final SpacecraftState[] initialStates;
138
139
140 private final RealMatrix jacobian;
141
142
143
144
145
146
147
148
149 protected AbstractBatchLSModel(final PropagatorBuilder[] propagatorBuilders,
150 final List<ObservedMeasurement<?>> measurements,
151 final ParameterDriversList estimatedMeasurementsParameters,
152 final ModelObserver observer) {
153
154 this.builders = propagatorBuilders.clone();
155 this.measurements = measurements;
156 this.estimatedMeasurementsParameters = estimatedMeasurementsParameters;
157 this.measurementParameterColumns = new HashMap<>(estimatedMeasurementsParameters.getNbValuesToEstimate());
158 this.estimatedOrbitalParameters = new ParameterDriversList[builders.length];
159 this.estimatedPropagationParameters = new ParameterDriversList[builders.length];
160 this.evaluations = new IdentityHashMap<>(measurements.size());
161 this.observer = observer;
162 this.harvesters = new MatricesHarvester[builders.length];
163 this.initialStates = new SpacecraftState[builders.length];
164
165
166 int rows = 0;
167 for (final ObservedMeasurement<?> measurement : measurements) {
168 rows += measurement.getDimension();
169 }
170
171 this.orbitsStartColumns = new int[builders.length];
172 this.orbitsEndColumns = new int[builders.length];
173 this.orbitsJacobianColumns = new int[builders.length * 6];
174 Arrays.fill(orbitsJacobianColumns, -1);
175 int columns = 0;
176 for (int i = 0; i < builders.length; ++i) {
177 this.orbitsStartColumns[i] = columns;
178 final List<ParameterDriversList.DelegatingDriver> orbitalParametersDrivers =
179 builders[i].getOrbitalParameterFactory().getOrbitalParametersDrivers().getDrivers();
180 for (int j = 0; j < orbitalParametersDrivers.size(); ++j) {
181 if (orbitalParametersDrivers.get(j).isSelected()) {
182 orbitsJacobianColumns[columns] = j;
183 ++columns;
184 }
185 }
186 this.orbitsEndColumns[i] = columns;
187 }
188
189
190 final List<String> estimatedPropagationParametersNames = new ArrayList<>();
191 for (int i = 0; i < builders.length; ++i) {
192
193
194 for (final DelegatingDriver delegating : getSelectedPropagationDriversForBuilder(i).getDrivers()) {
195
196 final TimeSpanMap<String> delegatingNameSpanMap = delegating.getNamesSpanMap();
197
198 Span<String> currentNameSpan = delegatingNameSpanMap.getFirstSpan();
199
200 if (!estimatedPropagationParametersNames.contains(currentNameSpan.getData())) {
201 estimatedPropagationParametersNames.add(currentNameSpan.getData());
202 }
203 for (int spanNumber = 1; spanNumber < delegatingNameSpanMap.getSpansNumber(); ++spanNumber) {
204 currentNameSpan = delegatingNameSpanMap.getSpan(currentNameSpan.getEnd());
205
206 if (!estimatedPropagationParametersNames.contains(currentNameSpan.getData())) {
207 estimatedPropagationParametersNames.add(currentNameSpan.getData());
208 }
209 }
210 }
211 }
212
213
214 propagationParameterColumns = new HashMap<>(estimatedPropagationParametersNames.size());
215 for (final String driverName : estimatedPropagationParametersNames) {
216 propagationParameterColumns.put(driverName, columns);
217 ++columns;
218 }
219
220 for (final ParameterDriver parameter : estimatedMeasurementsParameters.getDrivers()) {
221 for (Span<String> span = parameter.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
222 measurementParameterColumns.put(span.getData(), columns);
223 columns++;
224 }
225 }
226
227
228 value = new ArrayRealVector(rows);
229 jacobian = MatrixUtils.createRealMatrix(rows, columns);
230
231
232
233
234 final AbsoluteDate refDate = builders[0].getOrbitalParameterFactory().getDate();
235
236
237 measurements.sort(new ChronologicalComparator());
238 firstDate = measurements.getFirst().getDate();
239 lastDate = measurements.getLast().getDate();
240
241
242 forwardPropagation = FastMath.abs(refDate.durationFrom(firstDate)) <= FastMath.abs(refDate.durationFrom(lastDate));
243 }
244
245
246
247
248 public void setEvaluationsCounter(final Incrementor evaluationsCounter) {
249 this.evaluationsCounter = evaluationsCounter;
250 }
251
252
253
254
255 public void setIterationsCounter(final Incrementor iterationsCounter) {
256 this.iterationsCounter = iterationsCounter;
257 }
258
259
260
261
262 public boolean isForwardPropagation() {
263 return forwardPropagation;
264 }
265
266
267
268
269
270 protected abstract MatricesHarvester configureHarvester(Propagator propagator);
271
272
273
274
275
276
277
278
279
280 protected abstract Orbit configureOrbits(MatricesHarvester harvester, Propagator propagator);
281
282
283 @Override
284 public Pair<RealVector, RealMatrix> value(final RealVector point) {
285
286
287 final Propagator[] propagators = createPropagators(point);
288 final Orbit[] orbits = new Orbit[propagators.length];
289 for (int i = 0; i < propagators.length; ++i) {
290 harvesters[i] = configureHarvester(propagators[i]);
291 orbits[i] = configureOrbits(harvesters[i], propagators[i]);
292
293
294 initialStates[i] = propagators[i].getBaseInitialState();
295 }
296 final PropagatorsParallelizer parallelizer =
297 new PropagatorsParallelizer(Arrays.asList(propagators), configureMeasurements(point));
298
299
300 evaluations.clear();
301 value.set(0.0);
302 for (int i = 0; i < jacobian.getRowDimension(); ++i) {
303 for (int j = 0; j < jacobian.getColumnDimension(); ++j) {
304 jacobian.setEntry(i, j, 0.0);
305 }
306 }
307
308
309 if (isForwardPropagation()) {
310
311 parallelizer.propagate(firstDate.shiftedBy(-1.0), lastDate.shiftedBy(+1.0));
312 } else {
313
314 parallelizer.propagate(lastDate.shiftedBy(+1.0), firstDate.shiftedBy(-1.0));
315 }
316
317 observer.modelCalled(orbits, evaluations);
318
319 return new Pair<>(value, jacobian);
320
321 }
322
323
324
325
326
327
328 public ParameterDriversList getSelectedOrbitalParametersDriversForBuilder(final int iBuilder) {
329
330
331 if (estimatedOrbitalParameters[iBuilder] == null) {
332
333
334 final ParameterDriversList drivers = builders[iBuilder].
335 getOrbitalParameterFactory().
336 getOrbitalParametersDrivers();
337 final ParameterDriversList selectedOrbitalDrivers = new ParameterDriversList();
338 for (final DelegatingDriver delegating : drivers.getDrivers()) {
339 if (delegating.isSelected()) {
340 for (final ParameterDriver driver : delegating.getRawDrivers()) {
341 selectedOrbitalDrivers.add(driver);
342 }
343 }
344 }
345
346
347 estimatedOrbitalParameters[iBuilder] = selectedOrbitalDrivers;
348 }
349 return estimatedOrbitalParameters[iBuilder];
350 }
351
352
353
354
355
356 public ParameterDriversList getSelectedPropagationDriversForBuilder(final int iBuilder) {
357
358
359 if (estimatedPropagationParameters[iBuilder] == null) {
360
361
362 final ParameterDriversList selectedPropagationDrivers = new ParameterDriversList();
363 for (final DelegatingDriver delegating : builders[iBuilder].getPropagationParametersDrivers().getDrivers()) {
364 if (delegating.isSelected()) {
365 for (final ParameterDriver driver : delegating.getRawDrivers()) {
366 selectedPropagationDrivers.add(driver);
367 }
368 }
369 }
370
371
372
373 selectedPropagationDrivers.sort();
374
375
376 estimatedPropagationParameters[iBuilder] = selectedPropagationDrivers;
377 }
378 return estimatedPropagationParameters[iBuilder];
379 }
380
381
382
383
384
385 public Propagator[] createPropagators(final RealVector point) {
386
387 final Propagator[] propagators = new Propagator[builders.length];
388
389
390
391 for (int i = 0; i < builders.length; ++i) {
392
393 int element = 0;
394
395 final int nbOrb = orbitsEndColumns[i] - orbitsStartColumns[i];
396
397
398 final ParameterDriversList selectedPropagationDrivers = getSelectedPropagationDriversForBuilder(i);
399 final int nbParams = selectedPropagationDrivers.getNbParams();
400 final int nbValuesToEstimate = selectedPropagationDrivers.getNbValuesToEstimate();
401
402
403 final double[] propagatorArray = new double[nbOrb + nbValuesToEstimate];
404
405
406 for (int j = 0; j < nbOrb; ++j) {
407 propagatorArray[element++] = point.getEntry(orbitsStartColumns[i] + j);
408 }
409
410
411 for (int j = 0; j < nbParams; ++j) {
412 final DelegatingDriver driver = selectedPropagationDrivers.getDrivers().get(j);
413 final TimeSpanMap<String> delegatingNameSpanMap = driver.getNamesSpanMap();
414
415
416
417 Span<String> currentNameSpan = delegatingNameSpanMap.getFirstSpan();
418 propagatorArray[element++] = point.getEntry(propagationParameterColumns.get(currentNameSpan.getData()));
419
420 for (int spanNumber = 1; spanNumber < delegatingNameSpanMap.getSpansNumber(); ++spanNumber) {
421 currentNameSpan = delegatingNameSpanMap.getSpan(currentNameSpan.getEnd());
422 propagatorArray[element++] = point.getEntry(propagationParameterColumns.get(currentNameSpan.getData()));
423
424 }
425 }
426
427
428 propagators[i] = builders[i].buildPropagator(propagatorArray);
429 }
430
431 return propagators;
432
433 }
434
435
436
437
438
439 public void fetchEvaluatedMeasurement(final int index, final EstimatedMeasurement<?> evaluation) {
440
441
442 final SpacecraftState[] evaluationStates = evaluation.getStates();
443 final ObservedMeasurement<?> observedMeasurement = evaluation.getObservedMeasurement();
444
445
446 evaluations.put(observedMeasurement, evaluation);
447 if (evaluation.getStatus() == EstimatedMeasurementBase.Status.REJECTED) {
448 return;
449 }
450
451 final double[] evaluated = evaluation.getEstimatedValue();
452 final double[] observed = observedMeasurement.getObservedValue();
453 final double[] sigma = observedMeasurement.getTheoreticalStandardDeviation();
454 final double[] weight = evaluation.getObservedMeasurement().getBaseWeight();
455 for (int i = 0; i < evaluated.length; ++i) {
456 value.setEntry(index + i, weight[i] * (evaluated[i] - observed[i]) / sigma[i]);
457 }
458
459 for (int k = 0; k < evaluationStates.length; ++k) {
460
461 final int p = observedMeasurement.getSatellites().get(k).getPropagatorIndex();
462
463
464 final double[][] aCY = new double[6][6];
465 final Orbit currentOrbit = evaluationStates[k].getOrbit();
466 currentOrbit.getJacobianWrtParameters(builders[p].getOrbitalParameterFactory().getPositionAngleType(),
467 aCY);
468 final RealMatrix dCdY = new Array2DRowRealMatrix(aCY, false);
469
470
471 final RealMatrix dMdC = new Array2DRowRealMatrix(evaluation.getStateDerivatives(k), false);
472 final RealMatrix dMdY = dMdC.multiply(dCdY);
473
474
475 final ParameterDriversList selectedOrbitalDrivers = getSelectedOrbitalParametersDriversForBuilder(p);
476 final int nbOrbParams = selectedOrbitalDrivers.getNbParams();
477 if (nbOrbParams > 0) {
478 RealMatrix dYdY0 = harvesters[p].getStateTransitionMatrix(evaluationStates[k]);
479 if (dYdY0.getRowDimension() == 7) {
480
481 dYdY0 = dYdY0.getSubMatrix(0, 5, 0, 5);
482 }
483 final RealMatrix dMdY0 = dMdY.multiply(dYdY0);
484 final RealMatrix dY0dB0 = harvesters[p].getStateJacobianVsBuilderParameters(initialStates[p]);
485 final RealMatrix dMdB0 = dY0dB0 == null ? dMdY0 : dMdY0.multiply(dY0dB0);
486 for (int i = 0; i < dMdB0.getRowDimension(); ++i) {
487 for (int j = orbitsStartColumns[p]; j < orbitsEndColumns[p]; ++j) {
488 final ParameterDriver driver =
489 selectedOrbitalDrivers.getDrivers().get(j - orbitsStartColumns[p]);
490 final double partial = dMdB0.getEntry(i, orbitsJacobianColumns[j]);
491 jacobian.setEntry(index + i, j,
492 weight[i] * partial / sigma[i] * driver.getScale());
493 }
494 }
495 }
496
497
498 final ParameterDriversList selectedPropagationDrivers = getSelectedPropagationDriversForBuilder(p);
499 final int nbParams = selectedPropagationDrivers.getNbParams();
500 if (nbParams > 0) {
501 RealMatrix dYdPp = harvesters[p].getParametersJacobian(evaluationStates[k]);
502 if (dYdPp.getRowDimension() == 7) {
503
504 dYdPp = dYdPp.getSubMatrix(0, 5, 0, dYdPp.getColumnDimension() - 1);
505 }
506 final RealMatrix dMdPp = dMdY.multiply(dYdPp);
507
508 for (int i = 0; i < dMdPp.getRowDimension(); ++i) {
509 int col = 0;
510
511
512 for (int j = 0; j < nbParams; ++j) {
513 final ParameterDriver delegating = selectedPropagationDrivers.getDrivers().get(j);
514 final TimeSpanMap<String> delegatingNameSpanMap = delegating.getNamesSpanMap();
515
516 for (Span<String> currentNameSpan = delegatingNameSpanMap.getFirstSpan(); currentNameSpan != null; currentNameSpan = currentNameSpan.next()) {
517 jacobian.addToEntry(index + i, propagationParameterColumns.get(currentNameSpan.getData()),
518 weight[i] * dMdPp.getEntry(i, col++) / sigma[i] * delegating.getScale());
519 }
520 }
521 }
522 }
523 }
524
525 for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
526 if (driver.isSelected()) {
527 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
528 final double[] aMPm = evaluation.getParameterDerivatives(driver, span.getStart());
529 for (int i = 0; i < aMPm.length; ++i) {
530 jacobian.setEntry(index + i, measurementParameterColumns.get(span.getData()),
531 weight[i] * aMPm[i] / sigma[i] * driver.getScale());
532 }
533 }
534 }
535 }
536
537 }
538
539
540
541
542
543 private MultiSatStepHandler configureMeasurements(final RealVector point) {
544
545
546 int index = orbitsEndColumns[builders.length - 1] + propagationParameterColumns.size();
547 for (final ParameterDriver parameter : estimatedMeasurementsParameters.getDrivers()) {
548
549 for (Span<Double> span = parameter.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
550 parameter.setNormalizedValue(point.getEntry(index++), span.getStart());
551 }
552 }
553
554
555 final List<PreCompensation> precompensated = new ArrayList<>();
556 for (final ObservedMeasurement<?> measurement : measurements) {
557 if (measurement.isEnabled()) {
558 precompensated.add(new PreCompensation(measurement, evaluations.get(measurement)));
559 }
560 }
561 precompensated.sort(new ChronologicalComparator());
562
563
564 firstDate = precompensated.getFirst().getDate();
565 lastDate = precompensated.getLast().getDate();
566
567
568 if (!forwardPropagation) {
569 Collections.reverse(precompensated);
570 }
571
572 return new MeasurementHandler(this, precompensated);
573
574 }
575
576
577
578
579 public int getIterationsCount() {
580 return iterationsCounter.getCount();
581 }
582
583
584
585
586 public int getEvaluationsCount() {
587 return evaluationsCounter.getCount();
588 }
589
590 }