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 java.util.ArrayList;
20 import java.util.List;
21
22 import org.hipparchus.exception.LocalizedCoreFormats;
23 import org.hipparchus.geometry.euclidean.threed.Vector3D;
24 import org.hipparchus.linear.RealMatrix;
25 import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem.Evaluation;
26 import org.hipparchus.optim.nonlinear.vector.leastsquares.LevenbergMarquardtOptimizer;
27 import org.junit.Assert;
28 import org.junit.Test;
29 import org.orekit.attitudes.LofOffset;
30 import org.orekit.errors.OrekitException;
31 import org.orekit.errors.OrekitMessages;
32 import org.orekit.estimation.Context;
33 import org.orekit.estimation.EstimationTestUtils;
34 import org.orekit.estimation.Force;
35 import org.orekit.estimation.measurements.EstimationsProvider;
36 import org.orekit.estimation.measurements.GroundStation;
37 import org.orekit.estimation.measurements.InterSatellitesRangeMeasurementCreator;
38 import org.orekit.estimation.measurements.MultiplexedMeasurement;
39 import org.orekit.estimation.measurements.ObservedMeasurement;
40 import org.orekit.estimation.measurements.PVMeasurementCreator;
41 import org.orekit.estimation.measurements.Range;
42 import org.orekit.estimation.measurements.RangeMeasurementCreator;
43 import org.orekit.estimation.measurements.RangeRateMeasurementCreator;
44 import org.orekit.estimation.measurements.modifiers.OnBoardAntennaRangeModifier;
45 import org.orekit.forces.gravity.NewtonianAttraction;
46 import org.orekit.forces.radiation.RadiationSensitive;
47 import org.orekit.frames.LOFType;
48 import org.orekit.orbits.CartesianOrbit;
49 import org.orekit.orbits.KeplerianOrbit;
50 import org.orekit.orbits.Orbit;
51 import org.orekit.orbits.OrbitType;
52 import org.orekit.orbits.PositionAngle;
53 import org.orekit.propagation.BoundedPropagator;
54 import org.orekit.propagation.EphemerisGenerator;
55 import org.orekit.propagation.Propagator;
56 import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
57 import org.orekit.propagation.numerical.NumericalPropagator;
58 import org.orekit.time.AbsoluteDate;
59 import org.orekit.time.ChronologicalComparator;
60 import org.orekit.utils.ParameterDriver;
61 import org.orekit.utils.ParameterDriversList;
62 import org.orekit.utils.ParameterDriversList.DelegatingDriver;
63 import org.orekit.utils.TimeStampedPVCoordinates;
64
65 public class BatchLSEstimatorTest {
66
67
68
69
70 @Test
71 public void testKeplerPV() {
72
73 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
74
75 final NumericalPropagatorBuilder propagatorBuilder =
76 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
77 1.0e-6, 60.0, 1.0);
78
79
80 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
81 propagatorBuilder);
82 final List<ObservedMeasurement<?>> measurements =
83 EstimationTestUtils.createMeasurements(propagator,
84 new PVMeasurementCreator(),
85 0.0, 1.0, 300.0);
86
87
88 final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(),
89 propagatorBuilder);
90 for (final ObservedMeasurement<?> measurement : measurements) {
91 estimator.addMeasurement(measurement);
92 }
93 estimator.setParametersConvergenceThreshold(1.0e-2);
94 estimator.setMaxIterations(10);
95 estimator.setMaxEvaluations(20);
96
97 EstimationTestUtils.checkFit(context, estimator, 1, 4,
98 0.0, 2.2e-8,
99 0.0, 1.1e-7,
100 0.0, 1.4e-8,
101 0.0, 6.3e-12);
102
103 RealMatrix normalizedCovariances = estimator.getOptimum().getCovariances(1.0e-10);
104 RealMatrix physicalCovariances = estimator.getPhysicalCovariances(1.0e-10);
105 Assert.assertEquals(6, normalizedCovariances.getRowDimension());
106 Assert.assertEquals(6, normalizedCovariances.getColumnDimension());
107 Assert.assertEquals(6, physicalCovariances.getRowDimension());
108 Assert.assertEquals(6, physicalCovariances.getColumnDimension());
109 Assert.assertEquals(0.00258, physicalCovariances.getEntry(0, 0), 1.0e-5);
110
111 }
112
113
114 @Test
115 public void testKeplerPVBackward() {
116
117 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
118
119 final NumericalPropagatorBuilder propagatorBuilder =
120 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
121 1.0e-6, 60.0, 1.0);
122
123
124 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
125 propagatorBuilder);
126 final List<ObservedMeasurement<?>> measurements =
127 EstimationTestUtils.createMeasurements(propagator,
128 new PVMeasurementCreator(),
129 0.0, -1.0, 300.0);
130
131
132 final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(),
133 propagatorBuilder);
134 for (final ObservedMeasurement<?> measurement : measurements) {
135 estimator.addMeasurement(measurement);
136 }
137 estimator.setParametersConvergenceThreshold(1.0e-2);
138 estimator.setMaxIterations(10);
139 estimator.setMaxEvaluations(20);
140
141 EstimationTestUtils.checkFit(context, estimator, 1, 2,
142 0.0, 8.3e-9,
143 0.0, 5.3e-8,
144 0.0, 5.6e-9,
145 0.0, 1.6e-12);
146
147 RealMatrix normalizedCovariances = estimator.getOptimum().getCovariances(1.0e-10);
148 RealMatrix physicalCovariances = estimator.getPhysicalCovariances(1.0e-10);
149 Assert.assertEquals(6, normalizedCovariances.getRowDimension());
150 Assert.assertEquals(6, normalizedCovariances.getColumnDimension());
151 Assert.assertEquals(6, physicalCovariances.getRowDimension());
152 Assert.assertEquals(6, physicalCovariances.getColumnDimension());
153 Assert.assertEquals(0.00258, physicalCovariances.getEntry(0, 0), 1.0e-5);
154
155 }
156
157
158
159
160 @Test
161 public void testKeplerRange() {
162
163 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
164
165 final NumericalPropagatorBuilder propagatorBuilder =
166 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
167 1.0e-6, 60.0, 1.0);
168
169
170 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
171 propagatorBuilder);
172 final List<ObservedMeasurement<?>> measurements =
173 EstimationTestUtils.createMeasurements(propagator,
174 new RangeMeasurementCreator(context),
175 1.0, 3.0, 300.0);
176
177
178 final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(),
179 propagatorBuilder);
180 for (final ObservedMeasurement<?> range : measurements) {
181 estimator.addMeasurement(range);
182 }
183 estimator.setParametersConvergenceThreshold(1.0e-2);
184 estimator.setMaxIterations(10);
185 estimator.setMaxEvaluations(20);
186 estimator.setObserver(new BatchLSObserver() {
187 int lastIter = 0;
188 int lastEval = 0;
189
190 @Override
191 public void evaluationPerformed(int iterationsCount, int evaluationscount,
192 Orbit[] orbits,
193 ParameterDriversList estimatedOrbitalParameters,
194 ParameterDriversList estimatedPropagatorParameters,
195 ParameterDriversList estimatedMeasurementsParameters,
196 EstimationsProvider evaluationsProvider, Evaluation lspEvaluation) {
197 if (iterationsCount == lastIter) {
198 Assert.assertEquals(lastEval + 1, evaluationscount);
199 } else {
200 Assert.assertEquals(lastIter + 1, iterationsCount);
201 }
202 lastIter = iterationsCount;
203 lastEval = evaluationscount;
204 Assert.assertEquals(measurements.size(), evaluationsProvider.getNumber());
205 try {
206 evaluationsProvider.getEstimatedMeasurement(-1);
207 Assert.fail("an exception should have been thrown");
208 } catch (OrekitException oe) {
209 Assert.assertEquals(LocalizedCoreFormats.OUT_OF_RANGE_SIMPLE, oe.getSpecifier());
210 }
211 try {
212 evaluationsProvider.getEstimatedMeasurement(measurements.size());
213 Assert.fail("an exception should have been thrown");
214 } catch (OrekitException oe) {
215 Assert.assertEquals(LocalizedCoreFormats.OUT_OF_RANGE_SIMPLE, oe.getSpecifier());
216 }
217 AbsoluteDate previous = AbsoluteDate.PAST_INFINITY;
218 for (int i = 0; i < evaluationsProvider.getNumber(); ++i) {
219 AbsoluteDate current = evaluationsProvider.getEstimatedMeasurement(i).getDate();
220 Assert.assertTrue(current.compareTo(previous) >= 0);
221 previous = current;
222 }
223 }
224 });
225
226 ParameterDriver aDriver = estimator.getOrbitalParametersDrivers(true).getDrivers().get(0);
227 Assert.assertEquals("a", aDriver.getName());
228 aDriver.setValue(aDriver.getValue() + 1.2);
229 aDriver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
230
231 EstimationTestUtils.checkFit(context, estimator, 2, 3,
232 0.0, 1.1e-6,
233 0.0, 2.8e-6,
234 0.0, 4.0e-7,
235 0.0, 2.2e-10);
236
237
238
239 for (final ParameterDriver driver : estimator.getOrbitalParametersDrivers(true).getDrivers()) {
240 if ("a".equals(driver.getName())) {
241
242 Assert.assertEquals(0, driver.getReferenceDate().durationFrom(AbsoluteDate.GALILEO_EPOCH), 1.0e-15);
243 } else {
244
245 Assert.assertEquals(0, driver.getReferenceDate().durationFrom(propagatorBuilder.getInitialOrbitDate()), 1.0e-15);
246 }
247 }
248
249 }
250
251
252
253
254 @Test
255 public void testKeplerRangeWithOnBoardAntennaOffset() {
256
257 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
258
259 final NumericalPropagatorBuilder propagatorBuilder =
260 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
261 1.0e-6, 60.0, 1.0);
262 propagatorBuilder.setAttitudeProvider(new LofOffset(propagatorBuilder.getFrame(), LOFType.LVLH));
263 final Vector3D antennaPhaseCenter = new Vector3D(-1.2, 2.3, -0.7);
264
265
266 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
267 propagatorBuilder);
268 final List<ObservedMeasurement<?>> measurements =
269 EstimationTestUtils.createMeasurements(propagator,
270 new RangeMeasurementCreator(context, antennaPhaseCenter),
271 1.0, 3.0, 300.0);
272
273
274 final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(),
275 propagatorBuilder);
276 final OnBoardAntennaRangeModifier obaModifier = new OnBoardAntennaRangeModifier(antennaPhaseCenter);
277 for (final ObservedMeasurement<?> range : measurements) {
278 ((Range) range).addModifier(obaModifier);
279 estimator.addMeasurement(range);
280 }
281 estimator.setParametersConvergenceThreshold(1.0e-2);
282 estimator.setMaxIterations(10);
283 estimator.setMaxEvaluations(20);
284 estimator.setObserver(new BatchLSObserver() {
285 int lastIter = 0;
286 int lastEval = 0;
287
288 @Override
289 public void evaluationPerformed(int iterationsCount, int evaluationscount,
290 Orbit[] orbits,
291 ParameterDriversList estimatedOrbitalParameters,
292 ParameterDriversList estimatedPropagatorParameters,
293 ParameterDriversList estimatedMeasurementsParameters,
294 EstimationsProvider evaluationsProvider, Evaluation lspEvaluation) {
295 if (iterationsCount == lastIter) {
296 Assert.assertEquals(lastEval + 1, evaluationscount);
297 } else {
298 Assert.assertEquals(lastIter + 1, iterationsCount);
299 }
300 lastIter = iterationsCount;
301 lastEval = evaluationscount;
302 Assert.assertEquals(measurements.size(), evaluationsProvider.getNumber());
303 try {
304 evaluationsProvider.getEstimatedMeasurement(-1);
305 Assert.fail("an exception should have been thrown");
306 } catch (OrekitException oe) {
307 Assert.assertEquals(LocalizedCoreFormats.OUT_OF_RANGE_SIMPLE, oe.getSpecifier());
308 }
309 try {
310 evaluationsProvider.getEstimatedMeasurement(measurements.size());
311 Assert.fail("an exception should have been thrown");
312 } catch (OrekitException oe) {
313 Assert.assertEquals(LocalizedCoreFormats.OUT_OF_RANGE_SIMPLE, oe.getSpecifier());
314 }
315 AbsoluteDate previous = AbsoluteDate.PAST_INFINITY;
316 for (int i = 0; i < evaluationsProvider.getNumber(); ++i) {
317 AbsoluteDate current = evaluationsProvider.getEstimatedMeasurement(i).getDate();
318 Assert.assertTrue(current.compareTo(previous) >= 0);
319 previous = current;
320 }
321 }
322 });
323
324 ParameterDriver aDriver = estimator.getOrbitalParametersDrivers(true).getDrivers().get(0);
325 Assert.assertEquals("a", aDriver.getName());
326 aDriver.setValue(aDriver.getValue() + 1.2);
327 aDriver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
328
329 EstimationTestUtils.checkFit(context, estimator, 2, 3,
330 0.0, 2.0e-5,
331 0.0, 5.2e-5,
332 0.0, 2.7e-5,
333 0.0, 1.1e-8);
334
335
336
337 for (final ParameterDriver driver : estimator.getOrbitalParametersDrivers(true).getDrivers()) {
338 if ("a".equals(driver.getName())) {
339
340 Assert.assertEquals(0, driver.getReferenceDate().durationFrom(AbsoluteDate.GALILEO_EPOCH), 1.0e-15);
341 } else {
342
343 Assert.assertEquals(0, driver.getReferenceDate().durationFrom(propagatorBuilder.getInitialOrbitDate()), 1.0e-15);
344 }
345 }
346
347 }
348
349 @Test
350 public void testMultiSat() {
351
352 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
353
354 final NumericalPropagatorBuilder propagatorBuilder1 =
355 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
356 1.0e-6, 60.0, 1.0e-3);
357 final NumericalPropagatorBuilder propagatorBuilder2 =
358 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
359 1.0e-6, 60.0, 1.0e-3);
360
361
362 final TimeStampedPVCoordinates original = context.initialOrbit.getPVCoordinates();
363 final Orbit closeOrbit = new CartesianOrbit(new TimeStampedPVCoordinates(context.initialOrbit.getDate(),
364 original.getPosition().add(new Vector3D(1000, 2000, 3000)),
365 original.getVelocity().add(new Vector3D(-0.03, 0.01, 0.02))),
366 context.initialOrbit.getFrame(),
367 context.initialOrbit.getMu());
368 final Propagator closePropagator = EstimationTestUtils.createPropagator(closeOrbit,
369 propagatorBuilder2);
370 final EphemerisGenerator generator = closePropagator.getEphemerisGenerator();
371 closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
372 final BoundedPropagator ephemeris = generator.getGeneratedEphemeris();
373 Propagator propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit,
374 propagatorBuilder1);
375
376 final double localClockOffset = 0.137e-6;
377 final double remoteClockOffset = 469.0e-6;
378 final List<ObservedMeasurement<?>> r12 =
379 EstimationTestUtils.createMeasurements(propagator1,
380 new InterSatellitesRangeMeasurementCreator(ephemeris,
381 localClockOffset,
382 remoteClockOffset),
383 1.0, 3.0, 300.0);
384
385
386 propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit,
387 propagatorBuilder1);
388 final List<ObservedMeasurement<?>> r1 =
389 EstimationTestUtils.createMeasurements(propagator1,
390 new RangeMeasurementCreator(context),
391 1.0, 3.0, 300.0);
392
393
394 final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(),
395 propagatorBuilder1,
396 propagatorBuilder2);
397 for (final ObservedMeasurement<?> interSat : r12) {
398 estimator.addMeasurement(interSat);
399 }
400 for (final ObservedMeasurement<?> range : r1) {
401 estimator.addMeasurement(range);
402 }
403 estimator.setParametersConvergenceThreshold(1.0e-2);
404 estimator.setMaxIterations(10);
405 estimator.setMaxEvaluations(20);
406 estimator.setObserver(new BatchLSObserver() {
407 int lastIter = 0;
408 int lastEval = 0;
409
410 @Override
411 public void evaluationPerformed(int iterationsCount, int evaluationscount,
412 Orbit[] orbits,
413 ParameterDriversList estimatedOrbitalParameters,
414 ParameterDriversList estimatedPropagatorParameters,
415 ParameterDriversList estimatedMeasurementsParameters,
416 EstimationsProvider evaluationsProvider, Evaluation lspEvaluation) {
417 if (iterationsCount == lastIter) {
418 Assert.assertEquals(lastEval + 1, evaluationscount);
419 } else {
420 Assert.assertEquals(lastIter + 1, iterationsCount);
421 }
422 lastIter = iterationsCount;
423 lastEval = evaluationscount;
424 Assert.assertEquals(r12.size() + r1.size(), evaluationsProvider.getNumber());
425 try {
426 evaluationsProvider.getEstimatedMeasurement(-1);
427 Assert.fail("an exception should have been thrown");
428 } catch (OrekitException oe) {
429 Assert.assertEquals(LocalizedCoreFormats.OUT_OF_RANGE_SIMPLE, oe.getSpecifier());
430 }
431 try {
432 evaluationsProvider.getEstimatedMeasurement(r12.size() + r1.size());
433 Assert.fail("an exception should have been thrown");
434 } catch (OrekitException oe) {
435 Assert.assertEquals(LocalizedCoreFormats.OUT_OF_RANGE_SIMPLE, oe.getSpecifier());
436 }
437 AbsoluteDate previous = AbsoluteDate.PAST_INFINITY;
438 for (int i = 0; i < evaluationsProvider.getNumber(); ++i) {
439 AbsoluteDate current = evaluationsProvider.getEstimatedMeasurement(i).getDate();
440 Assert.assertTrue(current.compareTo(previous) >= 0);
441 previous = current;
442 }
443 }
444 });
445
446 List<DelegatingDriver> parameters = estimator.getOrbitalParametersDrivers(true).getDrivers();
447 ParameterDriver a0Driver = parameters.get(0);
448 Assert.assertEquals("a[0]", a0Driver.getName());
449 a0Driver.setValue(a0Driver.getValue() + 1.2);
450 a0Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
451
452 ParameterDriver a1Driver = parameters.get(6);
453 Assert.assertEquals("a[1]", a1Driver.getName());
454 a1Driver.setValue(a1Driver.getValue() - 5.4);
455 a1Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
456
457 final Orbit before = new KeplerianOrbit(parameters.get( 6).getValue(),
458 parameters.get( 7).getValue(),
459 parameters.get( 8).getValue(),
460 parameters.get( 9).getValue(),
461 parameters.get(10).getValue(),
462 parameters.get(11).getValue(),
463 PositionAngle.TRUE,
464 closeOrbit.getFrame(),
465 closeOrbit.getDate(),
466 closeOrbit.getMu());
467 Assert.assertEquals(4.7246,
468 Vector3D.distance(closeOrbit.getPVCoordinates().getPosition(),
469 before.getPVCoordinates().getPosition()),
470 1.0e-3);
471 Assert.assertEquals(0.0010514,
472 Vector3D.distance(closeOrbit.getPVCoordinates().getVelocity(),
473 before.getPVCoordinates().getVelocity()),
474 1.0e-6);
475 EstimationTestUtils.checkFit(context, estimator, 3, 4,
476 0.0, 2.9e-06,
477 0.0, 1.1e-05,
478 0.0, 8.3e-07,
479 0.0, 3.7e-10);
480
481 final Orbit determined = new KeplerianOrbit(parameters.get( 6).getValue(),
482 parameters.get( 7).getValue(),
483 parameters.get( 8).getValue(),
484 parameters.get( 9).getValue(),
485 parameters.get(10).getValue(),
486 parameters.get(11).getValue(),
487 PositionAngle.TRUE,
488 closeOrbit.getFrame(),
489 closeOrbit.getDate(),
490 closeOrbit.getMu());
491 Assert.assertEquals(0.0,
492 Vector3D.distance(closeOrbit.getPVCoordinates().getPosition(),
493 determined.getPVCoordinates().getPosition()),
494 2.8e-6);
495 Assert.assertEquals(0.0,
496 Vector3D.distance(closeOrbit.getPVCoordinates().getVelocity(),
497 determined.getPVCoordinates().getVelocity()),
498 1.6e-9);
499
500
501
502 for (final ParameterDriver driver : estimator.getOrbitalParametersDrivers(true).getDrivers()) {
503 if (driver.getName().startsWith("a[")) {
504
505 Assert.assertEquals(0, driver.getReferenceDate().durationFrom(AbsoluteDate.GALILEO_EPOCH), 1.0e-15);
506 } else {
507
508 Assert.assertEquals(0, driver.getReferenceDate().durationFrom(propagatorBuilder1.getInitialOrbitDate()), 1.0e-15);
509 }
510 }
511
512 }
513
514
515
516
517
518
519 @Test
520 public void testMultiSatWithParameters() {
521
522
523 final boolean muEstimated = true;
524 final boolean crEstimated1 = true;
525 final boolean caEstimated1 = true;
526 final boolean crEstimated2 = true;
527 final boolean caEstimated2 = false;
528
529
530
531 final Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
532 final NumericalPropagatorBuilder propagatorBuilder1 =
533 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
534 1.0e-6, 60.0, 1.0e-3, Force.POTENTIAL, Force.SOLAR_RADIATION_PRESSURE);
535
536
537 String satName = "sat 1";
538 for (DelegatingDriver driver:propagatorBuilder1.getPropagationParametersDrivers().getDrivers()) {
539 if (driver.getName().equals("central attraction coefficient")) {
540 driver.setSelected(muEstimated);
541 }
542 if (driver.getName().equals(RadiationSensitive.REFLECTION_COEFFICIENT)) {
543 driver.setName(driver.getName() + " " + satName);
544 driver.setSelected(crEstimated1);
545 }
546 if (driver.getName().equals(RadiationSensitive.ABSORPTION_COEFFICIENT)) {
547 driver.setName(driver.getName() + " " + satName);
548 driver.setSelected(caEstimated1);
549 }
550 }
551
552
553 final Context context2 = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
554 final NumericalPropagatorBuilder propagatorBuilder2 =
555 context2.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
556 1.0e-6, 60.0, 1.0e-3, Force.POTENTIAL, Force.SOLAR_RADIATION_PRESSURE);
557
558
559 satName = "sat 2";
560 for (ParameterDriver driver:propagatorBuilder2.getPropagationParametersDrivers().getDrivers()) {
561 if (driver.getName().equals("central attraction coefficient")) {
562 driver.setSelected(muEstimated);
563 }
564 if (driver.getName().equals(RadiationSensitive.REFLECTION_COEFFICIENT)) {
565 driver.setName(driver.getName() + " " + satName);
566 driver.setSelected(crEstimated2);
567 }
568 if (driver.getName().equals(RadiationSensitive.ABSORPTION_COEFFICIENT)) {
569 driver.setName(driver.getName() + " " + satName);
570 driver.setSelected(caEstimated2);
571 }
572 }
573
574
575 final TimeStampedPVCoordinates original = context.initialOrbit.getPVCoordinates();
576 final Orbit closeOrbit = new CartesianOrbit(new TimeStampedPVCoordinates(context.initialOrbit.getDate(),
577 original.getPosition().add(new Vector3D(1000, 2000, 3000)),
578 original.getVelocity().add(new Vector3D(-0.03, 0.01, 0.02))),
579 context.initialOrbit.getFrame(),
580 context.initialOrbit.getMu());
581 final Propagator closePropagator = EstimationTestUtils.createPropagator(closeOrbit,
582 propagatorBuilder2);
583 final EphemerisGenerator generator = closePropagator.getEphemerisGenerator();
584 closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
585 final BoundedPropagator ephemeris = generator.getGeneratedEphemeris();
586 Propagator propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit,
587 propagatorBuilder1);
588 final List<ObservedMeasurement<?>> r12 =
589 EstimationTestUtils.createMeasurements(propagator1,
590 new InterSatellitesRangeMeasurementCreator(ephemeris, 0., 0.),
591 1.0, 3.0, 300.0);
592
593
594 propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit,
595 propagatorBuilder1);
596 final List<ObservedMeasurement<?>> r1 =
597 EstimationTestUtils.createMeasurements(propagator1,
598 new RangeMeasurementCreator(context),
599 1.0, 3.0, 300.0);
600
601
602 final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(),
603 propagatorBuilder1,
604 propagatorBuilder2);
605 for (final ObservedMeasurement<?> interSat : r12) {
606 estimator.addMeasurement(interSat);
607 }
608 for (final ObservedMeasurement<?> range : r1) {
609 estimator.addMeasurement(range);
610 }
611 estimator.setParametersConvergenceThreshold(1.0e-3);
612 estimator.setMaxIterations(10);
613 estimator.setMaxEvaluations(20);
614 estimator.setObserver(new BatchLSObserver() {
615 int lastIter = 0;
616 int lastEval = 0;
617
618 @Override
619 public void evaluationPerformed(int iterationsCount, int evaluationscount,
620 Orbit[] orbits,
621 ParameterDriversList estimatedOrbitalParameters,
622 ParameterDriversList estimatedPropagatorParameters,
623 ParameterDriversList estimatedMeasurementsParameters,
624 EstimationsProvider evaluationsProvider, Evaluation lspEvaluation) {
625 if (iterationsCount == lastIter) {
626 Assert.assertEquals(lastEval + 1, evaluationscount);
627 } else {
628 Assert.assertEquals(lastIter + 1, iterationsCount);
629 }
630 lastIter = iterationsCount;
631 lastEval = evaluationscount;
632
633 AbsoluteDate previous = AbsoluteDate.PAST_INFINITY;
634 for (int i = 0; i < evaluationsProvider.getNumber(); ++i) {
635 AbsoluteDate current = evaluationsProvider.getEstimatedMeasurement(i).getDate();
636 Assert.assertTrue(current.compareTo(previous) >= 0);
637 previous = current;
638 }
639 }
640 });
641
642 List<DelegatingDriver> parameters = estimator.getOrbitalParametersDrivers(true).getDrivers();
643 ParameterDriver a0Driver = parameters.get(0);
644 Assert.assertEquals("a[0]", a0Driver.getName());
645 a0Driver.setValue(a0Driver.getValue() + 1.2);
646 a0Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
647
648 ParameterDriver a1Driver = parameters.get(6);
649 Assert.assertEquals("a[1]", a1Driver.getName());
650 a1Driver.setValue(a1Driver.getValue() - 5.4);
651 a1Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
652
653 final Orbit before = new KeplerianOrbit(parameters.get( 6).getValue(),
654 parameters.get( 7).getValue(),
655 parameters.get( 8).getValue(),
656 parameters.get( 9).getValue(),
657 parameters.get(10).getValue(),
658 parameters.get(11).getValue(),
659 PositionAngle.TRUE,
660 closeOrbit.getFrame(),
661 closeOrbit.getDate(),
662 closeOrbit.getMu());
663 Assert.assertEquals(4.7246,
664 Vector3D.distance(closeOrbit.getPVCoordinates().getPosition(),
665 before.getPVCoordinates().getPosition()),
666 1.0e-3);
667 Assert.assertEquals(0.0010514,
668 Vector3D.distance(closeOrbit.getPVCoordinates().getVelocity(),
669 before.getPVCoordinates().getVelocity()),
670 1.0e-6);
671 EstimationTestUtils.checkFit(context, estimator, 4, 5,
672 0.0, 7.0e-06,
673 0.0, 2.2e-05,
674 0.0, 6.8e-07,
675 0.0, 2.7e-10);
676
677 final Orbit determined = new KeplerianOrbit(parameters.get( 6).getValue(),
678 parameters.get( 7).getValue(),
679 parameters.get( 8).getValue(),
680 parameters.get( 9).getValue(),
681 parameters.get(10).getValue(),
682 parameters.get(11).getValue(),
683 PositionAngle.TRUE,
684 closeOrbit.getFrame(),
685 closeOrbit.getDate(),
686 closeOrbit.getMu());
687 Assert.assertEquals(0.0,
688 Vector3D.distance(closeOrbit.getPVCoordinates().getPosition(),
689 determined.getPVCoordinates().getPosition()),
690 5.4e-6);
691 Assert.assertEquals(0.0,
692 Vector3D.distance(closeOrbit.getPVCoordinates().getVelocity(),
693 determined.getPVCoordinates().getVelocity()),
694 4.1e-9);
695
696
697
698 for (final ParameterDriver driver : estimator.getOrbitalParametersDrivers(true).getDrivers()) {
699 if (driver.getName().startsWith("a[")) {
700
701 Assert.assertEquals(0, driver.getReferenceDate().durationFrom(AbsoluteDate.GALILEO_EPOCH), 1.0e-15);
702 } else {
703
704 Assert.assertEquals(0, driver.getReferenceDate().durationFrom(propagatorBuilder1.getInitialOrbitDate()), 1.0e-15);
705 }
706 }
707
708 }
709
710
711
712
713
714 @Test
715 public void testIssue617() {
716
717 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
718
719 final NumericalPropagatorBuilder propagatorBuilder1 =
720 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
721 1.0e-6, 60.0, 1.0);
722 final NumericalPropagatorBuilder propagatorBuilder2 =
723 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
724 1.0e-6, 60.0, 1.0);
725
726
727 final TimeStampedPVCoordinates original = context.initialOrbit.getPVCoordinates();
728 final Orbit closeOrbit = new CartesianOrbit(new TimeStampedPVCoordinates(context.initialOrbit.getDate(),
729 original.getPosition().add(new Vector3D(1000, 2000, 3000)),
730 original.getVelocity().add(new Vector3D(-0.03, 0.01, 0.02))),
731 context.initialOrbit.getFrame(),
732 context.initialOrbit.getMu());
733 final Propagator closePropagator = EstimationTestUtils.createPropagator(closeOrbit,
734 propagatorBuilder2);
735 final EphemerisGenerator generator = closePropagator.getEphemerisGenerator();
736 closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
737 final BoundedPropagator ephemeris = generator.getGeneratedEphemeris();
738 Propagator propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit,
739 propagatorBuilder1);
740
741 final List<ObservedMeasurement<?>> r12 =
742 EstimationTestUtils.createMeasurements(propagator1,
743 new InterSatellitesRangeMeasurementCreator(ephemeris, 0., 0.),
744 1.0, 3.0, 300.0);
745
746
747 propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit,
748 propagatorBuilder1);
749 final List<ObservedMeasurement<?>> r1 =
750 EstimationTestUtils.createMeasurements(propagator1,
751 new RangeMeasurementCreator(context),
752 1.0, 3.0, 300.0);
753
754
755 final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(),
756 propagatorBuilder1,
757 propagatorBuilder2);
758
759 final List<ObservedMeasurement<?>> independentMeasurements = new ArrayList<>();
760 independentMeasurements.addAll(r1);
761 independentMeasurements.addAll(r12);
762
763
764
765
766
767 final List<ObservedMeasurement<?>> multiplexed = multiplexMeasurements(independentMeasurements, 60.0);
768
769 for (final ObservedMeasurement<?> measurement : multiplexed) {
770 estimator.addMeasurement(measurement);
771 }
772
773 estimator.setParametersConvergenceThreshold(1.0e-2);
774 estimator.setMaxIterations(10);
775 estimator.setMaxEvaluations(20);
776 estimator.setObserver(new BatchLSObserver() {
777 int lastIter = 0;
778 int lastEval = 0;
779
780 @Override
781 public void evaluationPerformed(int iterationsCount, int evaluationscount,
782 Orbit[] orbits,
783 ParameterDriversList estimatedOrbitalParameters,
784 ParameterDriversList estimatedPropagatorParameters,
785 ParameterDriversList estimatedMeasurementsParameters,
786 EstimationsProvider evaluationsProvider, Evaluation lspEvaluation) {
787 if (iterationsCount == lastIter) {
788 Assert.assertEquals(lastEval + 1, evaluationscount);
789 } else {
790 Assert.assertEquals(lastIter + 1, iterationsCount);
791 }
792 lastIter = iterationsCount;
793 lastEval = evaluationscount;
794 Assert.assertEquals(multiplexed.size(), evaluationsProvider.getNumber());
795 try {
796 evaluationsProvider.getEstimatedMeasurement(-1);
797 Assert.fail("an exception should have been thrown");
798 } catch (OrekitException oe) {
799 Assert.assertEquals(LocalizedCoreFormats.OUT_OF_RANGE_SIMPLE, oe.getSpecifier());
800 }
801 try {
802 evaluationsProvider.getEstimatedMeasurement(r12.size() + r1.size());
803 Assert.fail("an exception should have been thrown");
804 } catch (OrekitException oe) {
805 Assert.assertEquals(LocalizedCoreFormats.OUT_OF_RANGE_SIMPLE, oe.getSpecifier());
806 }
807 AbsoluteDate previous = AbsoluteDate.PAST_INFINITY;
808 for (int i = 0; i < evaluationsProvider.getNumber(); ++i) {
809 AbsoluteDate current = evaluationsProvider.getEstimatedMeasurement(i).getDate();
810 Assert.assertTrue(current.compareTo(previous) >= 0);
811 previous = current;
812 }
813 }
814 });
815
816 List<DelegatingDriver> parameters = estimator.getOrbitalParametersDrivers(true).getDrivers();
817 ParameterDriver a0Driver = parameters.get(0);
818 Assert.assertEquals("a[0]", a0Driver.getName());
819 a0Driver.setValue(a0Driver.getValue() + 1.2);
820 a0Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
821
822 ParameterDriver a1Driver = parameters.get(6);
823 Assert.assertEquals("a[1]", a1Driver.getName());
824 a1Driver.setValue(a1Driver.getValue() - 5.4);
825 a1Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
826
827 final Orbit before = new KeplerianOrbit(parameters.get( 6).getValue(),
828 parameters.get( 7).getValue(),
829 parameters.get( 8).getValue(),
830 parameters.get( 9).getValue(),
831 parameters.get(10).getValue(),
832 parameters.get(11).getValue(),
833 PositionAngle.TRUE,
834 closeOrbit.getFrame(),
835 closeOrbit.getDate(),
836 closeOrbit.getMu());
837 Assert.assertEquals(4.7246,
838 Vector3D.distance(closeOrbit.getPVCoordinates().getPosition(),
839 before.getPVCoordinates().getPosition()),
840 1.0e-3);
841 Assert.assertEquals(0.0010514,
842 Vector3D.distance(closeOrbit.getPVCoordinates().getVelocity(),
843 before.getPVCoordinates().getVelocity()),
844 1.0e-6);
845 EstimationTestUtils.checkFit(context, estimator, 2, 3,
846 0.0, 2.9e-06,
847 0.0, 8.1e-06,
848 0.0, 7.1e-07,
849 0.0, 3.2e-10);
850
851 final Orbit determined = new KeplerianOrbit(parameters.get( 6).getValue(),
852 parameters.get( 7).getValue(),
853 parameters.get( 8).getValue(),
854 parameters.get( 9).getValue(),
855 parameters.get(10).getValue(),
856 parameters.get(11).getValue(),
857 PositionAngle.TRUE,
858 closeOrbit.getFrame(),
859 closeOrbit.getDate(),
860 closeOrbit.getMu());
861 Assert.assertEquals(0.0,
862 Vector3D.distance(closeOrbit.getPVCoordinates().getPosition(),
863 determined.getPVCoordinates().getPosition()),
864 3.3e-6);
865 Assert.assertEquals(0.0,
866 Vector3D.distance(closeOrbit.getPVCoordinates().getVelocity(),
867 determined.getPVCoordinates().getVelocity()),
868 1.6e-9);
869
870
871
872 for (final ParameterDriver driver : estimator.getOrbitalParametersDrivers(true).getDrivers()) {
873 if (driver.getName().startsWith("a[")) {
874
875 Assert.assertEquals(0, driver.getReferenceDate().durationFrom(AbsoluteDate.GALILEO_EPOCH), 1.0e-15);
876 } else {
877
878 Assert.assertEquals(0, driver.getReferenceDate().durationFrom(propagatorBuilder1.getInitialOrbitDate()), 1.0e-15);
879 }
880 }
881
882 }
883
884 @Test
885 public void testWrappedException() {
886
887 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
888
889 final NumericalPropagatorBuilder propagatorBuilder =
890 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
891 1.0e-6, 60.0, 1.0);
892
893
894 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
895 propagatorBuilder);
896 final List<ObservedMeasurement<?>> measurements =
897 EstimationTestUtils.createMeasurements(propagator,
898 new RangeMeasurementCreator(context),
899 1.0, 3.0, 300.0);
900
901
902 final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(),
903 propagatorBuilder);
904 for (final ObservedMeasurement<?> range : measurements) {
905 estimator.addMeasurement(range);
906 }
907 estimator.setParametersConvergenceThreshold(1.0e-2);
908 estimator.setMaxIterations(10);
909 estimator.setMaxEvaluations(20);
910 estimator.setObserver(new BatchLSObserver() {
911
912 @Override
913 public void evaluationPerformed(int iterationsCount, int evaluationscount,
914 Orbit[] orbits,
915 ParameterDriversList estimatedOrbitalParameters,
916 ParameterDriversList estimatedPropagatorParameters,
917 ParameterDriversList estimatedMeasurementsParameters,
918 EstimationsProvider evaluationsProvider, Evaluation lspEvaluation) {
919 throw new DummyException();
920 }
921 });
922
923 try {
924 EstimationTestUtils.checkFit(context, estimator, 3, 4,
925 0.0, 1.5e-6,
926 0.0, 3.2e-6,
927 0.0, 3.8e-7,
928 0.0, 1.5e-10);
929 Assert.fail("an exception should have been thrown");
930 } catch (DummyException de) {
931
932 }
933
934 }
935
936 private static class DummyException extends OrekitException {
937 private static final long serialVersionUID = 1L;
938 public DummyException() {
939 super(OrekitMessages.INTERNAL_ERROR);
940 }
941 }
942
943
944
945
946 @Test
947 public void testKeplerRangeRate() {
948
949 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
950
951 final NumericalPropagatorBuilder propagatorBuilder =
952 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
953 1.0e-6, 60.0, 1.0);
954
955
956 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
957 propagatorBuilder);
958 final double groundClockDrift = 4.8e-9;
959 for (final GroundStation station : context.stations) {
960 station.getClockDriftDriver().setValue(groundClockDrift);
961 }
962 final double satClkDrift = 3.2e-10;
963 final List<ObservedMeasurement<?>> measurements1 =
964 EstimationTestUtils.createMeasurements(propagator,
965 new RangeRateMeasurementCreator(context, false, satClkDrift),
966 1.0, 3.0, 300.0);
967
968 final List<ObservedMeasurement<?>> measurements = new ArrayList<ObservedMeasurement<?>>();
969 measurements.addAll(measurements1);
970
971
972 final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(),
973 propagatorBuilder);
974 for (final ObservedMeasurement<?> rangerate : measurements) {
975 estimator.addMeasurement(rangerate);
976 }
977 estimator.setParametersConvergenceThreshold(1.0e-3);
978 estimator.setMaxIterations(10);
979 estimator.setMaxEvaluations(20);
980
981 EstimationTestUtils.checkFit(context, estimator, 1, 2,
982 0.0, 5.3e-7,
983 0.0, 1.3e-6,
984 0.0, 8.4e-4,
985 0.0, 5.1e-7);
986 }
987
988
989
990
991 @Test
992 public void testKeplerRangeAndRangeRate() {
993
994 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
995
996 final NumericalPropagatorBuilder propagatorBuilder =
997 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
998 1.0e-6, 60.0, 1.0);
999
1000
1001 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
1002 propagatorBuilder);
1003
1004 final List<ObservedMeasurement<?>> measurementsRange =
1005 EstimationTestUtils.createMeasurements(propagator,
1006 new RangeMeasurementCreator(context),
1007 1.0, 3.0, 300.0);
1008 final double groundClockDrift = 4.8e-9;
1009 for (final GroundStation station : context.stations) {
1010 station.getClockDriftDriver().setValue(groundClockDrift);
1011 }
1012 final double satClkDrift = 3.2e-10;
1013 final List<ObservedMeasurement<?>> measurementsRangeRate =
1014 EstimationTestUtils.createMeasurements(propagator,
1015 new RangeRateMeasurementCreator(context, false, satClkDrift),
1016 1.0, 3.0, 300.0);
1017
1018
1019 final List<ObservedMeasurement<?>> measurements = new ArrayList<ObservedMeasurement<?>>();
1020 measurements.addAll(measurementsRange);
1021 measurements.addAll(measurementsRangeRate);
1022
1023
1024 final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(),
1025 propagatorBuilder);
1026 for (final ObservedMeasurement<?> meas : measurements) {
1027 estimator.addMeasurement(meas);
1028 }
1029 estimator.setParametersConvergenceThreshold(1.0e-3);
1030 estimator.setMaxIterations(10);
1031 estimator.setMaxEvaluations(20);
1032
1033
1034 EstimationTestUtils.checkFit(context, estimator, 1, 2,
1035 0.0, 4.6e7,
1036 0.0, 1.8e-6,
1037 0.0, 5.8e-7,
1038 0.0, 2.7e-10);
1039 }
1040
1041
1042
1043
1044 @Test
1045 public void testIssue359() {
1046
1047 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
1048
1049 final NumericalPropagatorBuilder propagatorBuilder =
1050 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
1051 1.0e-6, 60.0, 1.0);
1052
1053
1054
1055 propagatorBuilder.getPropagationParametersDrivers().getDrivers().get(0).setSelected(true);
1056
1057 final NumericalPropagator propagator = (NumericalPropagator) EstimationTestUtils.createPropagator(context.initialOrbit,
1058 propagatorBuilder);
1059 final List<ObservedMeasurement<?>> measurements =
1060 EstimationTestUtils.createMeasurements(propagator,
1061 new PVMeasurementCreator(),
1062 0.0, 1.0, 300.0);
1063
1064
1065 final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(),
1066 propagatorBuilder);
1067 for (final ObservedMeasurement<?> measurement : measurements) {
1068 estimator.addMeasurement(measurement);
1069 }
1070 ParameterDriversList estimatedParameters = estimator.getPropagatorParametersDrivers(true);
1071
1072 final String driverName = NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT;
1073 Assert.assertTrue(propagator.getAllForceModels().get(0) instanceof NewtonianAttraction);
1074 Assert.assertTrue(propagatorBuilder.getAllForceModels().get(0) instanceof NewtonianAttraction);
1075 Assert.assertNotNull(estimatedParameters.findByName(driverName));
1076 Assert.assertTrue(propagator.getAllForceModels().get(0).getParameterDriver(driverName).isSelected());
1077 Assert.assertTrue(propagatorBuilder.getAllForceModels().get(0).getParameterDriver(driverName).isSelected());
1078 }
1079
1080
1081
1082
1083
1084
1085 private List<ObservedMeasurement<?>> multiplexMeasurements(final List<ObservedMeasurement<?>> independentMeasurements,
1086 final double tol) {
1087 final List<ObservedMeasurement<?>> multiplexed = new ArrayList<>();
1088 independentMeasurements.sort(new ChronologicalComparator());
1089 List<ObservedMeasurement<?>> clump = new ArrayList<>();
1090 for (final ObservedMeasurement<?> measurement : independentMeasurements) {
1091 if (!clump.isEmpty() && measurement.getDate().durationFrom(clump.get(0).getDate()) > tol) {
1092
1093
1094 if (clump.size() == 1) {
1095 multiplexed.add(clump.get(0));
1096 } else {
1097 multiplexed.add(new MultiplexedMeasurement(clump));
1098 }
1099
1100
1101 clump = new ArrayList<>();
1102
1103 }
1104 clump.add(measurement);
1105 }
1106
1107 if (clump.size() == 1) {
1108 multiplexed.add(clump.get(0));
1109 } else {
1110 multiplexed.add(new MultiplexedMeasurement(clump));
1111 }
1112 return multiplexed;
1113 }
1114
1115 }
1116
1117