1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.common;
18
19 import java.io.BufferedReader;
20 import java.io.File;
21 import java.io.FileInputStream;
22 import java.io.IOException;
23 import java.io.InputStream;
24 import java.io.InputStreamReader;
25 import java.io.PrintStream;
26 import java.nio.charset.StandardCharsets;
27 import java.util.ArrayList;
28 import java.util.Arrays;
29 import java.util.Collections;
30 import java.util.Comparator;
31 import java.util.HashMap;
32 import java.util.List;
33 import java.util.Locale;
34 import java.util.Map;
35 import java.util.NoSuchElementException;
36 import java.util.regex.Pattern;
37
38 import org.hipparchus.exception.LocalizedCoreFormats;
39 import org.hipparchus.geometry.euclidean.threed.Vector3D;
40 import org.hipparchus.linear.MatrixUtils;
41 import org.hipparchus.linear.QRDecomposer;
42 import org.hipparchus.linear.RealMatrix;
43 import org.hipparchus.optim.nonlinear.vector.leastsquares.GaussNewtonOptimizer;
44 import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer;
45 import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem;
46 import org.hipparchus.optim.nonlinear.vector.leastsquares.LevenbergMarquardtOptimizer;
47 import org.hipparchus.util.FastMath;
48 import org.hipparchus.util.Precision;
49 import org.orekit.KeyValueFileParser;
50 import org.orekit.attitudes.AttitudeProvider;
51 import org.orekit.bodies.CelestialBody;
52 import org.orekit.bodies.CelestialBodyFactory;
53 import org.orekit.bodies.GeodeticPoint;
54 import org.orekit.bodies.OneAxisEllipsoid;
55 import org.orekit.data.DataContext;
56 import org.orekit.data.DataFilter;
57 import org.orekit.data.DataProvidersManager;
58 import org.orekit.data.GzipFilter;
59 import org.orekit.data.NamedData;
60 import org.orekit.data.UnixCompressFilter;
61 import org.orekit.errors.OrekitException;
62 import org.orekit.errors.OrekitMessages;
63 import org.orekit.estimation.leastsquares.BatchLSEstimator;
64 import org.orekit.estimation.leastsquares.BatchLSObserver;
65 import org.orekit.estimation.measurements.AngularAzEl;
66 import org.orekit.estimation.measurements.EstimatedMeasurement;
67 import org.orekit.estimation.measurements.EstimationsProvider;
68 import org.orekit.estimation.measurements.GroundStation;
69 import org.orekit.estimation.measurements.MultiplexedMeasurement;
70 import org.orekit.estimation.measurements.ObservableSatellite;
71 import org.orekit.estimation.measurements.ObservedMeasurement;
72 import org.orekit.estimation.measurements.PV;
73 import org.orekit.estimation.measurements.Range;
74 import org.orekit.estimation.measurements.RangeRate;
75 import org.orekit.estimation.measurements.modifiers.AngularRadioRefractionModifier;
76 import org.orekit.estimation.measurements.modifiers.Bias;
77 import org.orekit.estimation.measurements.modifiers.DynamicOutlierFilter;
78 import org.orekit.estimation.measurements.modifiers.OnBoardAntennaRangeModifier;
79 import org.orekit.estimation.measurements.modifiers.OutlierFilter;
80 import org.orekit.estimation.measurements.modifiers.RangeTroposphericDelayModifier;
81 import org.orekit.estimation.sequential.ConstantProcessNoise;
82 import org.orekit.estimation.sequential.KalmanEstimation;
83 import org.orekit.estimation.sequential.KalmanEstimator;
84 import org.orekit.estimation.sequential.KalmanEstimatorBuilder;
85 import org.orekit.estimation.sequential.KalmanObserver;
86 import org.orekit.forces.drag.DragSensitive;
87 import org.orekit.forces.drag.IsotropicDrag;
88 import org.orekit.forces.radiation.IsotropicRadiationSingleCoefficient;
89 import org.orekit.forces.radiation.RadiationSensitive;
90 import org.orekit.frames.EOPHistory;
91 import org.orekit.frames.Frame;
92 import org.orekit.frames.FramesFactory;
93 import org.orekit.frames.TopocentricFrame;
94 import org.orekit.gnss.HatanakaCompressFilter;
95 import org.orekit.gnss.MeasurementType;
96 import org.orekit.gnss.ObservationData;
97 import org.orekit.gnss.ObservationDataSet;
98 import org.orekit.gnss.RinexLoader;
99 import org.orekit.gnss.SatelliteSystem;
100 import org.orekit.models.AtmosphericRefractionModel;
101 import org.orekit.models.earth.EarthITU453AtmosphereRefraction;
102 import org.orekit.models.earth.atmosphere.Atmosphere;
103 import org.orekit.models.earth.atmosphere.DTM2000;
104 import org.orekit.models.earth.atmosphere.data.MarshallSolarActivityFutureEstimation;
105 import org.orekit.models.earth.displacement.OceanLoading;
106 import org.orekit.models.earth.displacement.OceanLoadingCoefficientsBLQFactory;
107 import org.orekit.models.earth.displacement.StationDisplacement;
108 import org.orekit.models.earth.displacement.TidalDisplacement;
109 import org.orekit.models.earth.troposphere.DiscreteTroposphericModel;
110 import org.orekit.models.earth.troposphere.EstimatedTroposphericModel;
111 import org.orekit.models.earth.troposphere.GlobalMappingFunctionModel;
112 import org.orekit.models.earth.troposphere.MappingFunction;
113 import org.orekit.models.earth.troposphere.NiellMappingFunctionModel;
114 import org.orekit.models.earth.troposphere.SaastamoinenModel;
115 import org.orekit.models.earth.weather.GlobalPressureTemperatureModel;
116 import org.orekit.orbits.CartesianOrbit;
117 import org.orekit.orbits.CircularOrbit;
118 import org.orekit.orbits.EquinoctialOrbit;
119 import org.orekit.orbits.KeplerianOrbit;
120 import org.orekit.orbits.Orbit;
121 import org.orekit.orbits.OrbitType;
122 import org.orekit.orbits.PositionAngle;
123 import org.orekit.propagation.Propagator;
124 import org.orekit.propagation.SpacecraftState;
125 import org.orekit.propagation.analytical.tle.TLE;
126 import org.orekit.propagation.analytical.tle.TLEPropagator;
127 import org.orekit.propagation.conversion.DormandPrince853IntegratorBuilder;
128 import org.orekit.propagation.conversion.IntegratedPropagatorBuilder;
129 import org.orekit.propagation.conversion.ODEIntegratorBuilder;
130 import org.orekit.time.AbsoluteDate;
131 import org.orekit.time.ChronologicalComparator;
132 import org.orekit.time.TimeScalesFactory;
133 import org.orekit.utils.Constants;
134 import org.orekit.utils.IERSConventions;
135 import org.orekit.utils.PVCoordinates;
136 import org.orekit.utils.ParameterDriver;
137 import org.orekit.utils.ParameterDriversList;
138 import org.orekit.utils.ParameterDriversList.DelegatingDriver;
139
140
141
142
143
144
145 public abstract class AbstractOrbitDetermination<T extends IntegratedPropagatorBuilder> {
146
147
148 private final String RANGE_BIAS_SUFFIX = "/range bias";
149
150
151 private final String RANGE_RATE_BIAS_SUFFIX = "/range rate bias";
152
153
154 private final String AZIMUTH_BIAS_SUFFIX = "/az bias";
155
156
157 private final String ELEVATION_BIAS_SUFFIX = "/el bias";
158
159
160
161
162
163 protected abstract void createGravityField(KeyValueFileParser<ParameterKey> parser)
164 throws NoSuchElementException;
165
166
167
168
169 protected abstract double getMu();
170
171
172
173
174
175
176
177
178
179
180
181
182
183 protected abstract T createPropagatorBuilder(Orbit referenceOrbit,
184 ODEIntegratorBuilder builder,
185 double positionScale);
186
187
188
189
190
191 protected abstract void setMass(T propagatorBuilder, double mass);
192
193
194
195
196
197
198 protected abstract ParameterDriver[] setGravity(T propagatorBuilder, OneAxisEllipsoid body);
199
200
201
202
203
204
205
206
207
208 protected abstract ParameterDriver[] setOceanTides(T propagatorBuilder, IERSConventions conventions,
209 OneAxisEllipsoid body, int degree, int order);
210
211
212
213
214
215
216
217
218 protected abstract ParameterDriver[]setSolidTides(T propagatorBuilder, IERSConventions conventions,
219 OneAxisEllipsoid body, CelestialBody[] solidTidesBodies);
220
221
222
223
224
225
226 protected abstract ParameterDriver[] setThirdBody(T propagatorBuilder, CelestialBody thirdBody);
227
228
229
230
231
232
233
234 protected abstract ParameterDriver[] setDrag(T propagatorBuilder, Atmosphere atmosphere, DragSensitive spacecraft);
235
236
237
238
239
240
241
242
243 protected abstract ParameterDriver[] setSolarRadiationPressure(T propagatorBuilder, CelestialBody sun,
244 double equatorialRadius, RadiationSensitive spacecraft);
245
246
247
248
249
250 protected abstract ParameterDriver[] setRelativity(T propagatorBuilder);
251
252
253
254
255
256
257
258
259 protected abstract ParameterDriver[] setPolynomialAcceleration(T propagatorBuilder, String name,
260 Vector3D direction, int degree);
261
262
263
264
265
266 protected abstract void setAttitudeProvider(T propagatorBuilder, AttitudeProvider attitudeProvider);
267
268
269
270
271
272
273 protected ResultBatchLeastSquares runBLS(final File input, final boolean print) throws IOException {
274
275
276 final KeyValueFileParser<ParameterKey> parser = new KeyValueFileParser<ParameterKey>(ParameterKey.class);
277 try (FileInputStream fis = new FileInputStream(input)) {
278 parser.parseInput(input.getAbsolutePath(), fis);
279 }
280
281 final RangeLogml#RangeLog">RangeLog rangeLog = new RangeLog();
282 final RangeRateLogg.html#RangeRateLog">RangeRateLog rangeRateLog = new RangeRateLog();
283 final AzimuthLoghtml#AzimuthLog">AzimuthLog azimuthLog = new AzimuthLog();
284 final ElevationLogg.html#ElevationLog">ElevationLog elevationLog = new ElevationLog();
285 final PositionLog.html#PositionLog">PositionLog positionLog = new PositionLog();
286 final VelocityLog.html#VelocityLog">VelocityLog velocityLog = new VelocityLog();
287
288
289 createGravityField(parser);
290
291
292 final Orbit initialGuess = createOrbit(parser, getMu());
293
294
295 final IERSConventions conventions;
296 if (!parser.containsKey(ParameterKey.IERS_CONVENTIONS)) {
297 conventions = IERSConventions.IERS_2010;
298 } else {
299 conventions = IERSConventions.valueOf("IERS_" + parser.getInt(ParameterKey.IERS_CONVENTIONS));
300 }
301
302
303 final OneAxisEllipsoid body = createBody(parser);
304
305
306 final T propagatorBuilder = configurePropagatorBuilder(parser, conventions, body, initialGuess);
307
308
309 final BatchLSEstimator estimator = createEstimator(parser, propagatorBuilder);
310
311 final Map<String, StationData> stations = createStationsData(parser, conventions, body);
312 final PVData pvData = createPVData(parser);
313 final ObservableSatellite satellite = createObservableSatellite(parser);
314 final Bias<Range> satRangeBias = createSatRangeBias(parser);
315 final OnBoardAntennaRangeModifier satAntennaRangeModifier = createSatAntennaRangeModifier(parser);
316 final Weights weights = createWeights(parser);
317 final OutlierFilter<Range> rangeOutliersManager = createRangeOutliersManager(parser, false);
318 final OutlierFilter<RangeRate> rangeRateOutliersManager = createRangeRateOutliersManager(parser, false);
319 final OutlierFilter<AngularAzEl> azElOutliersManager = createAzElOutliersManager(parser, false);
320 final OutlierFilter<PV> pvOutliersManager = createPVOutliersManager(parser, false);
321
322
323 final List<ObservedMeasurement<?>> independentMeasurements = new ArrayList<ObservedMeasurement<?>>();
324 for (final String fileName : parser.getStringsList(ParameterKey.MEASUREMENTS_FILES, ',')) {
325
326
327 NamedData nd = new NamedData(fileName, () -> new FileInputStream(new File(input.getParentFile(), fileName)));
328 for (final DataFilter filter : Arrays.asList(new GzipFilter(),
329 new UnixCompressFilter(),
330 new HatanakaCompressFilter())) {
331 nd = filter.filter(nd);
332 }
333
334 if (Pattern.matches(RinexLoader.DEFAULT_RINEX_2_SUPPORTED_NAMES, nd.getName()) ||
335 Pattern.matches(RinexLoader.DEFAULT_RINEX_3_SUPPORTED_NAMES, nd.getName())) {
336
337 independentMeasurements.addAll(readRinex(nd,
338 parser.getString(ParameterKey.SATELLITE_ID_IN_RINEX_FILES),
339 stations, satellite, satRangeBias, satAntennaRangeModifier, weights,
340 rangeOutliersManager, rangeRateOutliersManager));
341 } else {
342
343 independentMeasurements.addAll(readMeasurements(nd,
344 stations, pvData, satellite,
345 satRangeBias, satAntennaRangeModifier, weights,
346 rangeOutliersManager,
347 rangeRateOutliersManager,
348 azElOutliersManager,
349 pvOutliersManager));
350 }
351
352 }
353 final List<ObservedMeasurement<?>> multiplexed = multiplexMeasurements(independentMeasurements, 1.0e-9);
354 for (ObservedMeasurement<?> measurement : multiplexed) {
355 estimator.addMeasurement(measurement);
356 }
357
358
359 if (print) {
360 estimator.setObserver(new BatchLSObserver() {
361
362 private PVCoordinates previousPV;
363 {
364 previousPV = initialGuess.getPVCoordinates();
365 final String header = "iteration evaluations ΔP(m) ΔV(m/s) RMS nb Range nb Range-rate nb Angular nb PV%n";
366 System.out.format(Locale.US, header);
367 }
368
369
370 @Override
371 public void evaluationPerformed(final int iterationsCount, final int evaluationsCount,
372 final Orbit[] orbits,
373 final ParameterDriversList estimatedOrbitalParameters,
374 final ParameterDriversList estimatedPropagatorParameters,
375 final ParameterDriversList estimatedMeasurementsParameters,
376 final EstimationsProvider evaluationsProvider,
377 final LeastSquaresProblem.Evaluation lspEvaluation) {
378 final PVCoordinates currentPV = orbits[0].getPVCoordinates();
379 final String format0 = " %2d %2d %16.12f %s %s %s %s%n";
380 final String format = " %2d %2d %13.6f %12.9f %16.12f %s %s %s %s%n";
381 final EvaluationCounter<Range> rangeCounter = new EvaluationCounter<Range>();
382 final EvaluationCounter<RangeRate> rangeRateCounter = new EvaluationCounter<RangeRate>();
383 final EvaluationCounter<AngularAzEl> angularCounter = new EvaluationCounter<AngularAzEl>();
384 final EvaluationCounter<PV> pvCounter = new EvaluationCounter<PV>();
385 for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry : estimator.getLastEstimations().entrySet()) {
386 logEvaluation(entry.getValue(),
387 rangeCounter, rangeRateCounter, angularCounter, null, pvCounter, null);
388 }
389 if (evaluationsCount == 1) {
390 System.out.format(Locale.US, format0,
391 iterationsCount, evaluationsCount,
392 lspEvaluation.getRMS(),
393 rangeCounter.format(8), rangeRateCounter.format(8),
394 angularCounter.format(8), pvCounter.format(8));
395 } else {
396 System.out.format(Locale.US, format,
397 iterationsCount, evaluationsCount,
398 Vector3D.distance(previousPV.getPosition(), currentPV.getPosition()),
399 Vector3D.distance(previousPV.getVelocity(), currentPV.getVelocity()),
400 lspEvaluation.getRMS(),
401 rangeCounter.format(8), rangeRateCounter.format(8),
402 angularCounter.format(8), pvCounter.format(8));
403 }
404 previousPV = currentPV;
405 }
406 });
407 }
408 final Orbit estimated = estimator.estimate()[0].getInitialState().getOrbit();
409
410
411 for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry : estimator.getLastEstimations().entrySet()) {
412 logEvaluation(entry.getValue(),
413 rangeLog, rangeRateLog, azimuthLog, elevationLog, positionLog, velocityLog);
414 }
415
416 final ParameterDriversList propagatorParameters = estimator.getPropagatorParametersDrivers(true);
417 final ParameterDriversList measurementsParameters = estimator.getMeasurementsParametersDrivers(true);
418 return new ResultBatchLeastSquares(propagatorParameters, measurementsParameters,
419 estimator.getIterationsCount(), estimator.getEvaluationsCount(), estimated.getPVCoordinates(),
420 rangeLog.createStatisticsSummary(), rangeRateLog.createStatisticsSummary(),
421 azimuthLog.createStatisticsSummary(), elevationLog.createStatisticsSummary(),
422 positionLog.createStatisticsSummary(), velocityLog.createStatisticsSummary(),
423 estimator.getPhysicalCovariances(1.0e-10));
424
425 }
426
427
428
429
430
431
432
433
434
435
436
437
438
439 protected ResultKalman runKalman(final File input, final OrbitType orbitType, final boolean print,
440 final RealMatrix cartesianOrbitalP, final RealMatrix cartesianOrbitalQ,
441 final RealMatrix propagationP, final RealMatrix propagationQ,
442 final RealMatrix measurementP, final RealMatrix measurementQ)
443 throws IOException {
444
445
446 KeyValueFileParser<ParameterKey> parser = new KeyValueFileParser<ParameterKey>(ParameterKey.class);
447 parser.parseInput(input.getAbsolutePath(), new FileInputStream(input));
448
449
450 final RangeLogml#RangeLog">RangeLog rangeLog = new RangeLog();
451 final RangeRateLogg.html#RangeRateLog">RangeRateLog rangeRateLog = new RangeRateLog();
452 final AzimuthLoghtml#AzimuthLog">AzimuthLog azimuthLog = new AzimuthLog();
453 final ElevationLogg.html#ElevationLog">ElevationLog elevationLog = new ElevationLog();
454 final PositionLog.html#PositionLog">PositionLog positionLog = new PositionLog();
455 final VelocityLog.html#VelocityLog">VelocityLog velocityLog = new VelocityLog();
456
457
458 createGravityField(parser);
459
460
461 Orbit initialGuess = createOrbit(parser, getMu());
462
463
464 initialGuess = orbitType.convertType(initialGuess);
465
466
467 final IERSConventions conventions;
468 if (!parser.containsKey(ParameterKey.IERS_CONVENTIONS)) {
469 conventions = IERSConventions.IERS_2010;
470 } else {
471 conventions = IERSConventions.valueOf("IERS_" + parser.getInt(ParameterKey.IERS_CONVENTIONS));
472 }
473
474
475 final OneAxisEllipsoid body = createBody(parser);
476
477
478 final T propagatorBuilder =
479 configurePropagatorBuilder(parser, conventions, body, initialGuess);
480
481 final Map<String, StationData> stations = createStationsData(parser, conventions, body);
482 final PVData pvData = createPVData(parser);
483 final ObservableSatellite satellite = createObservableSatellite(parser);
484 final Bias<Range> satRangeBias = createSatRangeBias(parser);
485 final OnBoardAntennaRangeModifier satAntennaRangeModifier = createSatAntennaRangeModifier(parser);
486 final Weights weights = createWeights(parser);
487 final OutlierFilter<Range> rangeOutliersManager = createRangeOutliersManager(parser, true);
488 final OutlierFilter<RangeRate> rangeRateOutliersManager = createRangeRateOutliersManager(parser, true);
489 final OutlierFilter<AngularAzEl> azElOutliersManager = createAzElOutliersManager(parser, true);
490 final OutlierFilter<PV> pvOutliersManager = createPVOutliersManager(parser, true);
491
492
493 final List<ObservedMeasurement<?>> independentMeasurements = new ArrayList<ObservedMeasurement<?>>();
494 for (final String fileName : parser.getStringsList(ParameterKey.MEASUREMENTS_FILES, ',')) {
495
496
497 NamedData nd = new NamedData(fileName,
498 () -> new FileInputStream(new File(input.getParentFile(), fileName)));
499 for (final DataFilter filter : Arrays.asList(new GzipFilter(),
500 new UnixCompressFilter(),
501 new HatanakaCompressFilter())) {
502 nd = filter.filter(nd);
503 }
504
505 if (Pattern.matches(RinexLoader.DEFAULT_RINEX_2_SUPPORTED_NAMES, nd.getName()) ||
506 Pattern.matches(RinexLoader.DEFAULT_RINEX_3_SUPPORTED_NAMES, nd.getName())) {
507
508 independentMeasurements.addAll(readRinex(nd,
509 parser.getString(ParameterKey.SATELLITE_ID_IN_RINEX_FILES),
510 stations, satellite, satRangeBias, satAntennaRangeModifier, weights,
511 rangeOutliersManager, rangeRateOutliersManager));
512 } else {
513
514 independentMeasurements.addAll(readMeasurements(nd,
515 stations, pvData, satellite,
516 satRangeBias, satAntennaRangeModifier, weights,
517 rangeOutliersManager,
518 rangeRateOutliersManager,
519 azElOutliersManager,
520 pvOutliersManager));
521 }
522
523 }
524
525 final List<ObservedMeasurement<?>> multiplexed = multiplexMeasurements(independentMeasurements, 1.0e-9);
526
527
528
529
530
531
532
533
534 final ParameterDriversList estimatedMeasurementsParameters = new ParameterDriversList();
535 for (ObservedMeasurement<?> measurement : multiplexed) {
536 final List<ParameterDriver> drivers = measurement.getParametersDrivers();
537 for (ParameterDriver driver : drivers) {
538 if (driver.isSelected()) {
539
540 estimatedMeasurementsParameters.add(driver);
541 }
542 }
543 }
544
545 estimatedMeasurementsParameters.sort();
546
547
548
549 final double[][] dYdC = new double[6][6];
550 initialGuess.getJacobianWrtCartesian(propagatorBuilder.getPositionAngle(), dYdC);
551 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
552 RealMatrix orbitalP = Jac.multiply(cartesianOrbitalP.multiply(Jac.transpose()));
553
554
555 RealMatrix orbitalQ = Jac.multiply(cartesianOrbitalQ.multiply(Jac.transpose()));
556
557
558
559 final int nbPropag = (propagationP != null)?propagationP.getRowDimension():0;
560 final int nbMeas = (measurementP != null)?measurementP.getRowDimension():0;
561 final RealMatrix initialP = MatrixUtils.createRealMatrix(6 + nbPropag + nbMeas,
562 6 + nbPropag + nbMeas);
563 final RealMatrix Q = MatrixUtils.createRealMatrix(6 + nbPropag + nbMeas,
564 6 + nbPropag + nbMeas);
565
566 initialP.setSubMatrix(orbitalP.getData(), 0, 0);
567 Q.setSubMatrix(orbitalQ.getData(), 0, 0);
568
569
570 if (propagationP != null) {
571 initialP.setSubMatrix(propagationP.getData(), 6, 6);
572 Q.setSubMatrix(propagationQ.getData(), 6, 6);
573 }
574
575
576 if (measurementP != null) {
577 initialP.setSubMatrix(measurementP.getData(), 6 + nbPropag, 6 + nbPropag);
578 Q.setSubMatrix(measurementQ.getData(), 6 + nbPropag, 6 + nbPropag);
579 }
580
581
582 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
583 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
584 estimatedMeasurementsParameters(estimatedMeasurementsParameters).
585 build();
586
587
588 kalman.setObserver(new KalmanObserver() {
589
590
591 private AbsoluteDate t0;
592
593
594 @Override
595 @SuppressWarnings("unchecked")
596 public void evaluationPerformed(final KalmanEstimation estimation) {
597
598
599 final EstimatedMeasurement<?> estimatedMeasurement = estimation.getCorrectedMeasurement();
600 final int currentNumber = estimation.getCurrentMeasurementNumber();
601 final AbsoluteDate currentDate = estimatedMeasurement.getDate();
602 final EstimatedMeasurement.Status currentStatus = estimatedMeasurement.getStatus();
603
604
605 final ObservedMeasurement<?> observedMeasurement = estimatedMeasurement.getObservedMeasurement();
606
607
608 String measType = "";
609 String stationName = "";
610
611
612 logEvaluation(estimatedMeasurement,
613 rangeLog, rangeRateLog, azimuthLog, elevationLog, positionLog, velocityLog);
614 if (observedMeasurement instanceof Range) {
615 measType = "RANGE";
616 stationName = ((EstimatedMeasurement<Range>) estimatedMeasurement).getObservedMeasurement().
617 getStation().getBaseFrame().getName();
618 } else if (observedMeasurement instanceof RangeRate) {
619 measType = "RANGE_RATE";
620 stationName = ((EstimatedMeasurement<RangeRate>) estimatedMeasurement).getObservedMeasurement().
621 getStation().getBaseFrame().getName();
622 } else if (observedMeasurement instanceof AngularAzEl) {
623 measType = "AZ_EL";
624 stationName = ((EstimatedMeasurement<AngularAzEl>) estimatedMeasurement).getObservedMeasurement().
625 getStation().getBaseFrame().getName();
626 } else if (observedMeasurement instanceof PV) {
627 measType = "PV";
628 }
629
630
631
632
633
634 if (print) {
635 if (currentNumber == 1) {
636
637 t0 = currentDate;
638
639
640 final String formatHeader = "%-4s\t%-25s\t%15s\t%-10s\t%-10s\t%-20s\t%20s\t%20s";
641 String header = String.format(Locale.US, formatHeader,
642 "Nb", "Epoch", "Dt[s]", "Status", "Type", "Station",
643 "DP Corr", "DV Corr");
644
645 for (DelegatingDriver driver : estimation.getEstimatedOrbitalParameters().getDrivers()) {
646 header += String.format(Locale.US, "\t%20s", driver.getName());
647 header += String.format(Locale.US, "\t%20s", "D" + driver.getName());
648 }
649
650
651 for (DelegatingDriver driver : estimation.getEstimatedPropagationParameters().getDrivers()) {
652 header += String.format(Locale.US, "\t%20s", driver.getName());
653 header += String.format(Locale.US, "\t%20s", "D" + driver.getName());
654 }
655
656
657 for (DelegatingDriver driver : estimation.getEstimatedMeasurementsParameters().getDrivers()) {
658 header += String.format(Locale.US, "\t%20s", driver.getName());
659 header += String.format(Locale.US, "\t%20s", "D" + driver.getName());
660 }
661
662
663 System.out.println(header);
664 }
665
666
667 String line = "";
668
669 final String lineFormat = "%4d\t%-25s\t%15.3f\t%-10s\t%-10s\t%-20s\t%20.9e\t%20.9e";
670
671
672 final Vector3D predictedP = estimation.getPredictedSpacecraftStates()[0].getPVCoordinates().getPosition();
673 final Vector3D predictedV = estimation.getPredictedSpacecraftStates()[0].getPVCoordinates().getVelocity();
674 final Vector3D estimatedP = estimation.getCorrectedSpacecraftStates()[0].getPVCoordinates().getPosition();
675 final Vector3D estimatedV = estimation.getCorrectedSpacecraftStates()[0].getPVCoordinates().getVelocity();
676 final double DPcorr = Vector3D.distance(predictedP, estimatedP);
677 final double DVcorr = Vector3D.distance(predictedV, estimatedV);
678
679 line = String.format(Locale.US, lineFormat,
680 currentNumber, currentDate.toString(),
681 currentDate.durationFrom(t0), currentStatus.toString(),
682 measType, stationName,
683 DPcorr, DVcorr);
684
685
686 int jPar = 0;
687 final RealMatrix Pest = estimation.getPhysicalEstimatedCovarianceMatrix();
688
689 for (DelegatingDriver driver : estimation.getEstimatedOrbitalParameters().getDrivers()) {
690 line += String.format(Locale.US, "\t%20.9f", driver.getValue());
691 line += String.format(Locale.US, "\t%20.9e", FastMath.sqrt(Pest.getEntry(jPar, jPar)));
692 jPar++;
693 }
694
695 for (DelegatingDriver driver : estimation.getEstimatedPropagationParameters().getDrivers()) {
696 line += String.format(Locale.US, "\t%20.9f", driver.getValue());
697 line += String.format(Locale.US, "\t%20.9e", FastMath.sqrt(Pest.getEntry(jPar, jPar)));
698 jPar++;
699 }
700
701 for (DelegatingDriver driver : estimatedMeasurementsParameters.getDrivers()) {
702 line += String.format(Locale.US, "\t%20.9f", driver.getValue());
703 line += String.format(Locale.US, "\t%20.9e", FastMath.sqrt(Pest.getEntry(jPar, jPar)));
704 jPar++;
705 }
706
707
708 System.out.println(line);
709 }
710 }
711 });
712
713
714 final Orbit estimated = kalman.processMeasurements(multiplexed)[0].getInitialState().getOrbit();
715
716
717 final RealMatrix covarianceMatrix = kalman.getPhysicalEstimatedCovarianceMatrix();
718
719
720 final ParameterDriversList propagationParameters = kalman.getPropagationParametersDrivers(true);
721 final ParameterDriversList measurementsParameters = kalman.getEstimatedMeasurementsParameters();
722
723
724 if (print) {
725
726
727 int length = 0;
728 for (final ParameterDriver parameterDriver : propagationParameters.getDrivers()) {
729 length = FastMath.max(length, parameterDriver.getName().length());
730 }
731 for (final ParameterDriver parameterDriver : measurementsParameters.getDrivers()) {
732 length = FastMath.max(length, parameterDriver.getName().length());
733 }
734 if (propagationParameters.getNbParams() > 0) {
735 displayParametersChanges(System.out, "Estimated propagator parameters changes: ",
736 true, length, propagationParameters);
737 }
738 if (measurementsParameters.getNbParams() > 0) {
739 displayParametersChanges(System.out, "Estimated measurements parameters changes: ",
740 true, length, measurementsParameters);
741 }
742
743
744 System.out.println("");
745 rangeLog.displaySummary(System.out);
746 rangeRateLog.displaySummary(System.out);
747 azimuthLog.displaySummary(System.out);
748 elevationLog.displaySummary(System.out);
749 positionLog.displaySummary(System.out);
750 velocityLog.displaySummary(System.out);
751
752
753 displayFinalCovariances(System.out, kalman);
754 }
755
756
757 return new ResultKalman(propagationParameters, measurementsParameters,
758 kalman.getCurrentMeasurementNumber(), estimated.getPVCoordinates(),
759 rangeLog.createStatisticsSummary(), rangeRateLog.createStatisticsSummary(),
760 azimuthLog.createStatisticsSummary(), elevationLog.createStatisticsSummary(),
761 positionLog.createStatisticsSummary(), velocityLog.createStatisticsSummary(),
762 covarianceMatrix);
763 }
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778 protected Orbit runReference(final File input, final OrbitType orbitType,
779 final Vector3D refPosition, final Vector3D refVelocity,
780 final ParameterDriversList refPropagationParameters,
781 final AbsoluteDate finalDate) throws IOException {
782
783
784 KeyValueFileParser<ParameterKey> parser = new KeyValueFileParser<ParameterKey>(ParameterKey.class);
785 parser.parseInput(input.getAbsolutePath(), new FileInputStream(input));
786
787
788 createGravityField(parser);
789
790
791 Orbit initialRefOrbit = new CartesianOrbit(new PVCoordinates(refPosition, refVelocity),
792 parser.getInertialFrame(ParameterKey.INERTIAL_FRAME),
793 parser.getDate(ParameterKey.ORBIT_DATE,
794 TimeScalesFactory.getUTC()),
795 getMu());
796
797
798 initialRefOrbit = orbitType.convertType(initialRefOrbit);
799
800
801 final IERSConventions conventions;
802 if (!parser.containsKey(ParameterKey.IERS_CONVENTIONS)) {
803 conventions = IERSConventions.IERS_2010;
804 } else {
805 conventions = IERSConventions.valueOf("IERS_" + parser.getInt(ParameterKey.IERS_CONVENTIONS));
806 }
807
808
809 final OneAxisEllipsoid body = createBody(parser);
810
811
812 final T propagatorBuilder =
813 configurePropagatorBuilder(parser, conventions, body, initialRefOrbit);
814
815
816 if (refPropagationParameters != null) {
817 for (DelegatingDriver refDriver : refPropagationParameters.getDrivers()) {
818 for (DelegatingDriver driver : propagatorBuilder.getPropagationParametersDrivers().getDrivers()) {
819 if (driver.getName().equals(refDriver.getName())) {
820 driver.setValue(refDriver.getValue());
821 }
822 }
823 }
824 }
825
826
827 final Propagator propagator =
828 propagatorBuilder.buildPropagator(propagatorBuilder.
829 getSelectedNormalizedParameters());
830
831
832 return propagator.propagate(finalDate).getOrbit();
833
834 }
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849 private T configurePropagatorBuilder(final KeyValueFileParser<ParameterKey> parser,
850 final IERSConventions conventions,
851 final OneAxisEllipsoid body,
852 final Orbit orbit)
853 throws NoSuchElementException {
854
855 final double minStep;
856 if (!parser.containsKey(ParameterKey.PROPAGATOR_MIN_STEP)) {
857 minStep = 6000.0;
858 } else {
859 minStep = parser.getDouble(ParameterKey.PROPAGATOR_MIN_STEP);
860 }
861
862 final double maxStep;
863 if (!parser.containsKey(ParameterKey.PROPAGATOR_MAX_STEP)) {
864 maxStep = 86400;
865 } else {
866 maxStep = parser.getDouble(ParameterKey.PROPAGATOR_MAX_STEP);
867 }
868
869 final double dP;
870 if (!parser.containsKey(ParameterKey.PROPAGATOR_POSITION_ERROR)) {
871 dP = 10.0;
872 } else {
873 dP = parser.getDouble(ParameterKey.PROPAGATOR_POSITION_ERROR);
874 }
875
876 final double positionScale;
877 if (!parser.containsKey(ParameterKey.ESTIMATOR_ORBITAL_PARAMETERS_POSITION_SCALE)) {
878 positionScale = dP;
879 } else {
880 positionScale = parser.getDouble(ParameterKey.ESTIMATOR_ORBITAL_PARAMETERS_POSITION_SCALE);
881 }
882
883 final T propagatorBuilder = createPropagatorBuilder(orbit,
884 new DormandPrince853IntegratorBuilder(minStep, maxStep, dP),
885 positionScale);
886
887
888 final double mass;
889 if (!parser.containsKey(ParameterKey.MASS)) {
890 mass = 1000.0;
891 } else {
892 mass = parser.getDouble(ParameterKey.MASS);
893 }
894 setMass(propagatorBuilder, mass);
895
896 setGravity(propagatorBuilder, body);
897
898
899 if (parser.containsKey(ParameterKey.THIRD_BODY_SUN) &&
900 parser.getBoolean(ParameterKey.THIRD_BODY_SUN)) {
901 setThirdBody(propagatorBuilder, CelestialBodyFactory.getSun());
902 }
903 if (parser.containsKey(ParameterKey.THIRD_BODY_MOON) &&
904 parser.getBoolean(ParameterKey.THIRD_BODY_MOON)) {
905 setThirdBody(propagatorBuilder, CelestialBodyFactory.getMoon());
906 }
907
908
909 if (parser.containsKey(ParameterKey.OCEAN_TIDES_DEGREE) &&
910 parser.containsKey(ParameterKey.OCEAN_TIDES_ORDER)) {
911 final int degree = parser.getInt(ParameterKey.OCEAN_TIDES_DEGREE);
912 final int order = parser.getInt(ParameterKey.OCEAN_TIDES_ORDER);
913 if (degree > 0 && order > 0) {
914 setOceanTides(propagatorBuilder, conventions, body, degree, order);
915 }
916 }
917
918
919 final List<CelestialBody> solidTidesBodies = new ArrayList<CelestialBody>();
920 if (parser.containsKey(ParameterKey.SOLID_TIDES_SUN) &&
921 parser.getBoolean(ParameterKey.SOLID_TIDES_SUN)) {
922 solidTidesBodies.add(CelestialBodyFactory.getSun());
923 }
924 if (parser.containsKey(ParameterKey.SOLID_TIDES_MOON) &&
925 parser.getBoolean(ParameterKey.SOLID_TIDES_MOON)) {
926 solidTidesBodies.add(CelestialBodyFactory.getMoon());
927 }
928 if (!solidTidesBodies.isEmpty()) {
929 setSolidTides(propagatorBuilder, conventions, body,
930 solidTidesBodies.toArray(new CelestialBody[solidTidesBodies.size()]));
931 }
932
933
934 if (parser.containsKey(ParameterKey.DRAG) && parser.getBoolean(ParameterKey.DRAG)) {
935 final double cd = parser.getDouble(ParameterKey.DRAG_CD);
936 final double area = parser.getDouble(ParameterKey.DRAG_AREA);
937 final boolean cdEstimated = parser.getBoolean(ParameterKey.DRAG_CD_ESTIMATED);
938
939 final MarshallSolarActivityFutureEstimation msafe =
940 new MarshallSolarActivityFutureEstimation(MarshallSolarActivityFutureEstimation.DEFAULT_SUPPORTED_NAMES,
941 MarshallSolarActivityFutureEstimation.StrengthLevel.AVERAGE);
942 final DataProvidersManager manager = DataContext.getDefault().getDataProvidersManager();
943 manager.feed(msafe.getSupportedNames(), msafe);
944 final Atmosphere atmosphere = new DTM2000(msafe, CelestialBodyFactory.getSun(), body);
945 final ParameterDriver[] drivers = setDrag(propagatorBuilder, atmosphere, new IsotropicDrag(area, cd));
946 if (cdEstimated) {
947 for (final ParameterDriver driver : drivers) {
948 if (driver.getName().equals(DragSensitive.DRAG_COEFFICIENT)) {
949 driver.setSelected(true);
950 }
951 }
952 }
953 }
954
955
956 if (parser.containsKey(ParameterKey.SOLAR_RADIATION_PRESSURE) && parser.getBoolean(ParameterKey.SOLAR_RADIATION_PRESSURE)) {
957 final double cr = parser.getDouble(ParameterKey.SOLAR_RADIATION_PRESSURE_CR);
958 final double area = parser.getDouble(ParameterKey.SOLAR_RADIATION_PRESSURE_AREA);
959 final boolean cREstimated = parser.getBoolean(ParameterKey.SOLAR_RADIATION_PRESSURE_CR_ESTIMATED);
960 final ParameterDriver[] drivers = setSolarRadiationPressure(propagatorBuilder, CelestialBodyFactory.getSun(),
961 body.getEquatorialRadius(),
962 new IsotropicRadiationSingleCoefficient(area, cr));
963 if (cREstimated) {
964 for (final ParameterDriver driver : drivers) {
965 if (driver.getName().equals(RadiationSensitive.REFLECTION_COEFFICIENT)) {
966 driver.setSelected(true);
967 }
968 }
969 }
970 }
971
972
973 if (parser.containsKey(ParameterKey.GENERAL_RELATIVITY) && parser.getBoolean(ParameterKey.GENERAL_RELATIVITY)) {
974 setRelativity(propagatorBuilder);
975 }
976
977
978 if (parser.containsKey(ParameterKey.POLYNOMIAL_ACCELERATION_NAME)) {
979 final String[] names = parser.getStringArray(ParameterKey.POLYNOMIAL_ACCELERATION_NAME);
980 final Vector3D[] directions = parser.getVectorArray(ParameterKey.POLYNOMIAL_ACCELERATION_DIRECTION_X,
981 ParameterKey.POLYNOMIAL_ACCELERATION_DIRECTION_Y,
982 ParameterKey.POLYNOMIAL_ACCELERATION_DIRECTION_Z);
983 final List<String>[] coefficients = parser.getStringsListArray(ParameterKey.POLYNOMIAL_ACCELERATION_COEFFICIENTS, ',');
984 final boolean[] estimated = parser.getBooleanArray(ParameterKey.POLYNOMIAL_ACCELERATION_ESTIMATED);
985
986 for (int i = 0; i < names.length; ++i) {
987 final ParameterDriver[] drivers = setPolynomialAcceleration(propagatorBuilder, names[i], directions[i], coefficients[i].size() - 1);
988 for (int k = 0; k < coefficients[i].size(); ++k) {
989 final String coefficientName = names[i] + "[" + k + "]";
990 for (final ParameterDriver driver : drivers) {
991 if (driver.getName().equals(coefficientName)) {
992 driver.setValue(Double.parseDouble(coefficients[i].get(k)));
993 driver.setSelected(estimated[i]);
994 }
995 }
996 }
997 }
998 }
999
1000
1001 final AttitudeMode mode;
1002 if (parser.containsKey(ParameterKey.ATTITUDE_MODE)) {
1003 mode = AttitudeMode.valueOf(parser.getString(ParameterKey.ATTITUDE_MODE));
1004 } else {
1005 mode = AttitudeMode.NADIR_POINTING_WITH_YAW_COMPENSATION;
1006 }
1007 setAttitudeProvider(propagatorBuilder, mode.getProvider(orbit.getFrame(), body));
1008
1009 return propagatorBuilder;
1010
1011 }
1012
1013
1014
1015
1016
1017
1018 private OneAxisEllipsoid createBody(final KeyValueFileParser<ParameterKey> parser)
1019 throws NoSuchElementException {
1020
1021 final Frame bodyFrame;
1022 if (!parser.containsKey(ParameterKey.BODY_FRAME)) {
1023 bodyFrame = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
1024 } else {
1025 bodyFrame = parser.getEarthFrame(ParameterKey.BODY_FRAME);
1026 }
1027
1028 final double equatorialRadius;
1029 if (!parser.containsKey(ParameterKey.BODY_EQUATORIAL_RADIUS)) {
1030 equatorialRadius = Constants.WGS84_EARTH_EQUATORIAL_RADIUS;
1031 } else {
1032 equatorialRadius = parser.getDouble(ParameterKey.BODY_EQUATORIAL_RADIUS);
1033 }
1034
1035 final double flattening;
1036 if (!parser.containsKey(ParameterKey.BODY_INVERSE_FLATTENING)) {
1037 flattening = Constants.WGS84_EARTH_FLATTENING;
1038 } else {
1039 flattening = 1.0 / parser.getDouble(ParameterKey.BODY_INVERSE_FLATTENING);
1040 }
1041
1042 return new OneAxisEllipsoid(equatorialRadius, flattening, bodyFrame);
1043 }
1044
1045
1046
1047
1048
1049
1050
1051 private Orbit createOrbit(final KeyValueFileParser<ParameterKey> parser, final double mu)
1052 throws NoSuchElementException {
1053
1054 final Frame frame;
1055 if (!parser.containsKey(ParameterKey.INERTIAL_FRAME)) {
1056 frame = FramesFactory.getEME2000();
1057 } else {
1058 frame = parser.getInertialFrame(ParameterKey.INERTIAL_FRAME);
1059 }
1060
1061
1062 PositionAngle angleType = PositionAngle.MEAN;
1063 if (parser.containsKey(ParameterKey.ORBIT_ANGLE_TYPE)) {
1064 angleType = PositionAngle.valueOf(parser.getString(ParameterKey.ORBIT_ANGLE_TYPE).toUpperCase());
1065 }
1066 if (parser.containsKey(ParameterKey.ORBIT_KEPLERIAN_A)) {
1067 return new KeplerianOrbit(parser.getDouble(ParameterKey.ORBIT_KEPLERIAN_A),
1068 parser.getDouble(ParameterKey.ORBIT_KEPLERIAN_E),
1069 parser.getAngle(ParameterKey.ORBIT_KEPLERIAN_I),
1070 parser.getAngle(ParameterKey.ORBIT_KEPLERIAN_PA),
1071 parser.getAngle(ParameterKey.ORBIT_KEPLERIAN_RAAN),
1072 parser.getAngle(ParameterKey.ORBIT_KEPLERIAN_ANOMALY),
1073 angleType,
1074 frame,
1075 parser.getDate(ParameterKey.ORBIT_DATE,
1076 TimeScalesFactory.getUTC()),
1077 mu);
1078 } else if (parser.containsKey(ParameterKey.ORBIT_EQUINOCTIAL_A)) {
1079 return new EquinoctialOrbit(parser.getDouble(ParameterKey.ORBIT_EQUINOCTIAL_A),
1080 parser.getDouble(ParameterKey.ORBIT_EQUINOCTIAL_EX),
1081 parser.getDouble(ParameterKey.ORBIT_EQUINOCTIAL_EY),
1082 parser.getDouble(ParameterKey.ORBIT_EQUINOCTIAL_HX),
1083 parser.getDouble(ParameterKey.ORBIT_EQUINOCTIAL_HY),
1084 parser.getAngle(ParameterKey.ORBIT_EQUINOCTIAL_LAMBDA),
1085 angleType,
1086 frame,
1087 parser.getDate(ParameterKey.ORBIT_DATE,
1088 TimeScalesFactory.getUTC()),
1089 mu);
1090 } else if (parser.containsKey(ParameterKey.ORBIT_CIRCULAR_A)) {
1091 return new CircularOrbit(parser.getDouble(ParameterKey.ORBIT_CIRCULAR_A),
1092 parser.getDouble(ParameterKey.ORBIT_CIRCULAR_EX),
1093 parser.getDouble(ParameterKey.ORBIT_CIRCULAR_EY),
1094 parser.getAngle(ParameterKey.ORBIT_CIRCULAR_I),
1095 parser.getAngle(ParameterKey.ORBIT_CIRCULAR_RAAN),
1096 parser.getAngle(ParameterKey.ORBIT_CIRCULAR_ALPHA),
1097 angleType,
1098 frame,
1099 parser.getDate(ParameterKey.ORBIT_DATE,
1100 TimeScalesFactory.getUTC()),
1101 mu);
1102 } else if (parser.containsKey(ParameterKey.ORBIT_TLE_LINE_1)) {
1103 final String line1 = parser.getString(ParameterKey.ORBIT_TLE_LINE_1);
1104 final String line2 = parser.getString(ParameterKey.ORBIT_TLE_LINE_2);
1105 final TLE tle = new TLE(line1, line2);
1106
1107 final TLEPropagator propagator = TLEPropagator.selectExtrapolator(tle);
1108
1109 final AbsoluteDate initDate = tle.getDate();
1110 final SpacecraftState initialState = propagator.getInitialState();
1111
1112
1113
1114 return new CartesianOrbit(initialState.getPVCoordinates(FramesFactory.getEME2000()),
1115 frame,
1116 initDate,
1117 mu);
1118
1119
1120 } else {
1121 final double[] pos = {parser.getDouble(ParameterKey.ORBIT_CARTESIAN_PX),
1122 parser.getDouble(ParameterKey.ORBIT_CARTESIAN_PY),
1123 parser.getDouble(ParameterKey.ORBIT_CARTESIAN_PZ)};
1124 final double[] vel = {parser.getDouble(ParameterKey.ORBIT_CARTESIAN_VX),
1125 parser.getDouble(ParameterKey.ORBIT_CARTESIAN_VY),
1126 parser.getDouble(ParameterKey.ORBIT_CARTESIAN_VZ)};
1127
1128 return new CartesianOrbit(new PVCoordinates(new Vector3D(pos), new Vector3D(vel)),
1129 frame,
1130 parser.getDate(ParameterKey.ORBIT_DATE,
1131 TimeScalesFactory.getUTC()),
1132 mu);
1133 }
1134 }
1135
1136
1137
1138
1139
1140 private Bias<Range> createSatRangeBias(final KeyValueFileParser<ParameterKey> parser) {
1141
1142
1143 final double transponderDelayBias;
1144 if (!parser.containsKey(ParameterKey.ONBOARD_RANGE_BIAS)) {
1145 transponderDelayBias = 0;
1146 } else {
1147 transponderDelayBias = parser.getDouble(ParameterKey.ONBOARD_RANGE_BIAS);
1148 }
1149
1150 final double transponderDelayBiasMin;
1151 if (!parser.containsKey(ParameterKey.ONBOARD_RANGE_BIAS_MIN)) {
1152 transponderDelayBiasMin = Double.NEGATIVE_INFINITY;
1153 } else {
1154 transponderDelayBiasMin = parser.getDouble(ParameterKey.ONBOARD_RANGE_BIAS_MIN);
1155 }
1156
1157 final double transponderDelayBiasMax;
1158 if (!parser.containsKey(ParameterKey.ONBOARD_RANGE_BIAS_MAX)) {
1159 transponderDelayBiasMax = Double.NEGATIVE_INFINITY;
1160 } else {
1161 transponderDelayBiasMax = parser.getDouble(ParameterKey.ONBOARD_RANGE_BIAS_MAX);
1162 }
1163
1164
1165 final boolean transponderDelayBiasEstimated;
1166 if (!parser.containsKey(ParameterKey.ONBOARD_RANGE_BIAS_ESTIMATED)) {
1167 transponderDelayBiasEstimated = false;
1168 } else {
1169 transponderDelayBiasEstimated = parser.getBoolean(ParameterKey.ONBOARD_RANGE_BIAS_ESTIMATED);
1170 }
1171
1172 if (FastMath.abs(transponderDelayBias) >= Precision.SAFE_MIN || transponderDelayBiasEstimated) {
1173
1174
1175 final Bias<Range> bias = new Bias<Range>(new String[] { "transponder delay bias", },
1176 new double[] { transponderDelayBias },
1177 new double[] { 1.0 },
1178 new double[] { transponderDelayBiasMin },
1179 new double[] { transponderDelayBiasMax });
1180 bias.getParametersDrivers().get(0).setSelected(transponderDelayBiasEstimated);
1181 return bias;
1182 } else {
1183
1184 return null;
1185 }
1186 }
1187
1188
1189
1190
1191
1192 private OnBoardAntennaRangeModifier createSatAntennaRangeModifier(final KeyValueFileParser<ParameterKey> parser) {
1193 final Vector3D offset;
1194 if (!parser.containsKey(ParameterKey.ON_BOARD_ANTENNA_PHASE_CENTER_X)) {
1195 offset = Vector3D.ZERO;
1196 } else {
1197 offset = parser.getVector(ParameterKey.ON_BOARD_ANTENNA_PHASE_CENTER_X,
1198 ParameterKey.ON_BOARD_ANTENNA_PHASE_CENTER_Y,
1199 ParameterKey.ON_BOARD_ANTENNA_PHASE_CENTER_Z);
1200 }
1201 return offset.getNorm() > 0 ? new OnBoardAntennaRangeModifier(offset) : null;
1202 }
1203
1204
1205
1206
1207
1208
1209
1210
1211 private Map<String, StationData> createStationsData(final KeyValueFileParser<ParameterKey> parser,
1212 final IERSConventions conventions,
1213 final OneAxisEllipsoid body)
1214 throws NoSuchElementException {
1215
1216 final Map<String, StationData> stations = new HashMap<String, StationData>();
1217
1218 final String[] stationNames = parser.getStringArray(ParameterKey.GROUND_STATION_NAME);
1219 final double[] stationLatitudes = parser.getAngleArray(ParameterKey.GROUND_STATION_LATITUDE);
1220 final double[] stationLongitudes = parser.getAngleArray(ParameterKey.GROUND_STATION_LONGITUDE);
1221 final double[] stationAltitudes = parser.getDoubleArray(ParameterKey.GROUND_STATION_ALTITUDE);
1222 final boolean[] stationPositionEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_POSITION_ESTIMATED);
1223 final double[] stationClockOffsets = parser.getDoubleArray(ParameterKey.GROUND_STATION_CLOCK_OFFSET);
1224 final double[] stationClockOffsetsMin = parser.getDoubleArray(ParameterKey.GROUND_STATION_CLOCK_OFFSET_MIN);
1225 final double[] stationClockOffsetsMax = parser.getDoubleArray(ParameterKey.GROUND_STATION_CLOCK_OFFSET_MAX);
1226 final boolean[] stationClockOffsetEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_CLOCK_OFFSET_ESTIMATED);
1227 final double[] stationRangeSigma = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_SIGMA);
1228 final double[] stationRangeBias = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_BIAS);
1229 final double[] stationRangeBiasMin = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_BIAS_MIN);
1230 final double[] stationRangeBiasMax = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_BIAS_MAX);
1231 final boolean[] stationRangeBiasEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_RANGE_BIAS_ESTIMATED);
1232 final double[] stationRangeRateSigma = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_SIGMA);
1233 final double[] stationRangeRateBias = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS);
1234 final double[] stationRangeRateBiasMin = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS_MIN);
1235 final double[] stationRangeRateBiasMax = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS_MAX);
1236 final boolean[] stationRangeRateBiasEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS_ESTIMATED);
1237 final double[] stationAzimuthSigma = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_SIGMA);
1238 final double[] stationAzimuthBias = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_BIAS);
1239 final double[] stationAzimuthBiasMin = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_BIAS_MIN);
1240 final double[] stationAzimuthBiasMax = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_BIAS_MAX);
1241 final double[] stationElevationSigma = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_SIGMA);
1242 final double[] stationElevationBias = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_BIAS);
1243 final double[] stationElevationBiasMin = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_BIAS_MIN);
1244 final double[] stationElevationBiasMax = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_BIAS_MAX);
1245 final boolean[] stationAzElBiasesEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_AZ_EL_BIASES_ESTIMATED);
1246 final boolean[] stationElevationRefraction = parser.getBooleanArray(ParameterKey.GROUND_STATION_ELEVATION_REFRACTION_CORRECTION);
1247 final boolean[] stationTroposphericModelEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_TROPOSPHERIC_MODEL_ESTIMATED);
1248 final double[] stationTroposphericZenithDelay = parser.getDoubleArray(ParameterKey.GROUND_STATION_TROPOSPHERIC_ZENITH_DELAY);
1249 final boolean[] stationZenithDelayEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_TROPOSPHERIC_DELAY_ESTIMATED);
1250 final boolean[] stationGlobalMappingFunction = parser.getBooleanArray(ParameterKey.GROUND_STATION_GLOBAL_MAPPING_FUNCTION);
1251 final boolean[] stationNiellMappingFunction = parser.getBooleanArray(ParameterKey.GROUND_STATION_NIELL_MAPPING_FUNCTION);
1252 final boolean[] stationWeatherEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_WEATHER_ESTIMATED);
1253 final boolean[] stationRangeTropospheric = parser.getBooleanArray(ParameterKey.GROUND_STATION_RANGE_TROPOSPHERIC_CORRECTION);
1254
1255
1256 final TidalDisplacement tidalDisplacement;
1257 if (parser.containsKey(ParameterKey.SOLID_TIDES_DISPLACEMENT_CORRECTION) &&
1258 parser.getBoolean(ParameterKey.SOLID_TIDES_DISPLACEMENT_CORRECTION)) {
1259 final boolean removePermanentDeformation =
1260 parser.containsKey(ParameterKey.SOLID_TIDES_DISPLACEMENT_REMOVE_PERMANENT_DEFORMATION) &&
1261 parser.getBoolean(ParameterKey.SOLID_TIDES_DISPLACEMENT_REMOVE_PERMANENT_DEFORMATION);
1262 tidalDisplacement = new TidalDisplacement(Constants.EIGEN5C_EARTH_EQUATORIAL_RADIUS,
1263 Constants.JPL_SSD_SUN_EARTH_PLUS_MOON_MASS_RATIO,
1264 Constants.JPL_SSD_EARTH_MOON_MASS_RATIO,
1265 CelestialBodyFactory.getSun(),
1266 CelestialBodyFactory.getMoon(),
1267 conventions,
1268 removePermanentDeformation);
1269 } else {
1270 tidalDisplacement = null;
1271 }
1272
1273 final OceanLoadingCoefficientsBLQFactory blqFactory;
1274 if (parser.containsKey(ParameterKey.OCEAN_LOADING_CORRECTION) &&
1275 parser.getBoolean(ParameterKey.OCEAN_LOADING_CORRECTION)) {
1276 blqFactory = new OceanLoadingCoefficientsBLQFactory("^.*\\.blq$");
1277 } else {
1278 blqFactory = null;
1279 }
1280
1281 final EOPHistory eopHistory = FramesFactory.findEOP(body.getBodyFrame());
1282
1283 for (int i = 0; i < stationNames.length; ++i) {
1284
1285
1286 final StationDisplacement[] displacements;
1287 final OceanLoading oceanLoading = (blqFactory == null) ?
1288 null :
1289 new OceanLoading(body, blqFactory.getCoefficients(stationNames[i]));
1290 if (tidalDisplacement == null) {
1291 if (oceanLoading == null) {
1292 displacements = new StationDisplacement[0];
1293 } else {
1294 displacements = new StationDisplacement[] {
1295 oceanLoading
1296 };
1297 }
1298 } else {
1299 if (oceanLoading == null) {
1300 displacements = new StationDisplacement[] {
1301 tidalDisplacement
1302 };
1303 } else {
1304 displacements = new StationDisplacement[] {
1305 tidalDisplacement, oceanLoading
1306 };
1307 }
1308 }
1309
1310
1311 final GeodeticPoint position = new GeodeticPoint(stationLatitudes[i],
1312 stationLongitudes[i],
1313 stationAltitudes[i]);
1314 final TopocentricFrame topo = new TopocentricFrame(body, position, stationNames[i]);
1315 final GroundStation station = new GroundStation(topo, eopHistory, displacements);
1316 station.getClockOffsetDriver().setReferenceValue(stationClockOffsets[i]);
1317 station.getClockOffsetDriver().setValue(stationClockOffsets[i]);
1318 station.getClockOffsetDriver().setMinValue(stationClockOffsetsMin[i]);
1319 station.getClockOffsetDriver().setMaxValue(stationClockOffsetsMax[i]);
1320 station.getClockOffsetDriver().setSelected(stationClockOffsetEstimated[i]);
1321 station.getEastOffsetDriver().setSelected(stationPositionEstimated[i]);
1322 station.getNorthOffsetDriver().setSelected(stationPositionEstimated[i]);
1323 station.getZenithOffsetDriver().setSelected(stationPositionEstimated[i]);
1324
1325
1326 final double rangeSigma = stationRangeSigma[i];
1327 final Bias<Range> rangeBias;
1328 if (FastMath.abs(stationRangeBias[i]) >= Precision.SAFE_MIN || stationRangeBiasEstimated[i]) {
1329 rangeBias = new Bias<Range>(new String[] { stationNames[i] + RANGE_BIAS_SUFFIX, },
1330 new double[] { stationRangeBias[i] },
1331 new double[] { rangeSigma },
1332 new double[] { stationRangeBiasMin[i] },
1333 new double[] { stationRangeBiasMax[i] });
1334 rangeBias.getParametersDrivers().get(0).setSelected(stationRangeBiasEstimated[i]);
1335 } else {
1336
1337 rangeBias = null;
1338 }
1339
1340
1341 final double rangeRateSigma = stationRangeRateSigma[i];
1342 final Bias<RangeRate> rangeRateBias;
1343 if (FastMath.abs(stationRangeRateBias[i]) >= Precision.SAFE_MIN || stationRangeRateBiasEstimated[i]) {
1344 rangeRateBias = new Bias<RangeRate>(new String[] { stationNames[i] + RANGE_RATE_BIAS_SUFFIX },
1345 new double[] { stationRangeRateBias[i] },
1346 new double[] { rangeRateSigma },
1347 new double[] { stationRangeRateBiasMin[i] },
1348 new double[] {
1349 stationRangeRateBiasMax[i]
1350 });
1351 rangeRateBias.getParametersDrivers().get(0).setSelected(stationRangeRateBiasEstimated[i]);
1352 } else {
1353
1354 rangeRateBias = null;
1355 }
1356
1357
1358 final double[] azELSigma = new double[] {
1359 stationAzimuthSigma[i], stationElevationSigma[i]
1360 };
1361 final Bias<AngularAzEl> azELBias;
1362 if (FastMath.abs(stationAzimuthBias[i]) >= Precision.SAFE_MIN ||
1363 FastMath.abs(stationElevationBias[i]) >= Precision.SAFE_MIN ||
1364 stationAzElBiasesEstimated[i]) {
1365 azELBias = new Bias<AngularAzEl>(new String[] { stationNames[i] + AZIMUTH_BIAS_SUFFIX,
1366 stationNames[i] + ELEVATION_BIAS_SUFFIX },
1367 new double[] { stationAzimuthBias[i], stationElevationBias[i] },
1368 azELSigma,
1369 new double[] { stationAzimuthBiasMin[i], stationElevationBiasMin[i] },
1370 new double[] { stationAzimuthBiasMax[i], stationElevationBiasMax[i] });
1371 azELBias.getParametersDrivers().get(0).setSelected(stationAzElBiasesEstimated[i]);
1372 azELBias.getParametersDrivers().get(1).setSelected(stationAzElBiasesEstimated[i]);
1373 } else {
1374
1375 azELBias = null;
1376 }
1377
1378
1379 final AngularRadioRefractionModifier refractionCorrection;
1380 if (stationElevationRefraction[i]) {
1381 final double altitude = station.getBaseFrame().getPoint().getAltitude();
1382 final AtmosphericRefractionModel refractionModel = new EarthITU453AtmosphereRefraction(altitude);
1383 refractionCorrection = new AngularRadioRefractionModifier(refractionModel);
1384 } else {
1385 refractionCorrection = null;
1386 }
1387
1388
1389
1390 final RangeTroposphericDelayModifier rangeTroposphericCorrection;
1391 if (stationRangeTropospheric[i]) {
1392
1393 MappingFunction mappingModel = null;
1394 if (stationGlobalMappingFunction[i]) {
1395 mappingModel = new GlobalMappingFunctionModel(stationLatitudes[i],
1396 stationLongitudes[i]);
1397 } else if (stationNiellMappingFunction[i]) {
1398 mappingModel = new NiellMappingFunctionModel(stationLatitudes[i]);
1399 }
1400
1401 final DiscreteTroposphericModel troposphericModel;
1402 if (stationTroposphericModelEstimated[i] && mappingModel != null) {
1403
1404 if (stationWeatherEstimated[i]) {
1405 final GlobalPressureTemperatureModel weather = new GlobalPressureTemperatureModel(stationLatitudes[i],
1406 stationLongitudes[i],
1407 body.getBodyFrame());
1408 weather.weatherParameters(stationAltitudes[i], parser.getDate(ParameterKey.ORBIT_DATE,
1409 TimeScalesFactory.getUTC()));
1410 final double temperature = weather.getTemperature();
1411 final double pressure = weather.getPressure();
1412 troposphericModel = new EstimatedTroposphericModel(temperature, pressure, mappingModel,
1413 stationTroposphericZenithDelay[i]);
1414 } else {
1415 troposphericModel = new EstimatedTroposphericModel(mappingModel, stationTroposphericZenithDelay[i]);
1416 }
1417
1418 final ParameterDriver totalDelay = troposphericModel.getParametersDrivers().get(0);
1419 totalDelay.setSelected(stationZenithDelayEstimated[i]);
1420 totalDelay.setName(stationNames[i].substring(0, 5) + EstimatedTroposphericModel.TOTAL_ZENITH_DELAY);
1421 } else {
1422 troposphericModel = SaastamoinenModel.getStandardModel();
1423 }
1424
1425 rangeTroposphericCorrection = new RangeTroposphericDelayModifier(troposphericModel);
1426 } else {
1427 rangeTroposphericCorrection = null;
1428 }
1429
1430
1431 stations.put(stationNames[i],
1432 new StationData(station,
1433 rangeSigma, rangeBias,
1434 rangeRateSigma, rangeRateBias,
1435 azELSigma, azELBias,
1436 refractionCorrection, rangeTroposphericCorrection));
1437 }
1438 return stations;
1439
1440 }
1441
1442
1443
1444
1445
1446
1447 private Weights createWeights(final KeyValueFileParser<ParameterKey> parser)
1448 throws NoSuchElementException {
1449 return new Weights(parser.getDouble(ParameterKey.RANGE_MEASUREMENTS_BASE_WEIGHT),
1450 parser.getDouble(ParameterKey.RANGE_RATE_MEASUREMENTS_BASE_WEIGHT),
1451 new double[] {
1452 parser.getDouble(ParameterKey.AZIMUTH_MEASUREMENTS_BASE_WEIGHT),
1453 parser.getDouble(ParameterKey.ELEVATION_MEASUREMENTS_BASE_WEIGHT)
1454 },
1455 parser.getDouble(ParameterKey.PV_MEASUREMENTS_BASE_WEIGHT));
1456 }
1457
1458
1459
1460
1461
1462
1463 private OutlierFilter<Range> createRangeOutliersManager(final KeyValueFileParser<ParameterKey> parser, boolean isDynamic) {
1464 needsBothOrNeither(parser,
1465 ParameterKey.RANGE_OUTLIER_REJECTION_MULTIPLIER,
1466 ParameterKey.RANGE_OUTLIER_REJECTION_STARTING_ITERATION);
1467 return isDynamic ?
1468 new DynamicOutlierFilter<>(parser.getInt(ParameterKey.RANGE_OUTLIER_REJECTION_STARTING_ITERATION),
1469 parser.getInt(ParameterKey.RANGE_OUTLIER_REJECTION_MULTIPLIER)) :
1470 new OutlierFilter<>(parser.getInt(ParameterKey.RANGE_OUTLIER_REJECTION_STARTING_ITERATION),
1471 parser.getInt(ParameterKey.RANGE_OUTLIER_REJECTION_MULTIPLIER));
1472 }
1473
1474
1475
1476
1477
1478
1479 private OutlierFilter<RangeRate> createRangeRateOutliersManager(final KeyValueFileParser<ParameterKey> parser, boolean isDynamic) {
1480 needsBothOrNeither(parser,
1481 ParameterKey.RANGE_RATE_OUTLIER_REJECTION_MULTIPLIER,
1482 ParameterKey.RANGE_RATE_OUTLIER_REJECTION_STARTING_ITERATION);
1483 return isDynamic ?
1484 new DynamicOutlierFilter<>(parser.getInt(ParameterKey.RANGE_RATE_OUTLIER_REJECTION_STARTING_ITERATION),
1485 parser.getInt(ParameterKey.RANGE_RATE_OUTLIER_REJECTION_MULTIPLIER)) :
1486 new OutlierFilter<>(parser.getInt(ParameterKey.RANGE_RATE_OUTLIER_REJECTION_STARTING_ITERATION),
1487 parser.getInt(ParameterKey.RANGE_RATE_OUTLIER_REJECTION_MULTIPLIER));
1488 }
1489
1490
1491
1492
1493
1494
1495 private OutlierFilter<AngularAzEl> createAzElOutliersManager(final KeyValueFileParser<ParameterKey> parser, boolean isDynamic) {
1496 needsBothOrNeither(parser,
1497 ParameterKey.AZ_EL_OUTLIER_REJECTION_MULTIPLIER,
1498 ParameterKey.AZ_EL_OUTLIER_REJECTION_STARTING_ITERATION);
1499 return isDynamic ?
1500 new DynamicOutlierFilter<>(parser.getInt(ParameterKey.AZ_EL_OUTLIER_REJECTION_STARTING_ITERATION),
1501 parser.getInt(ParameterKey.AZ_EL_OUTLIER_REJECTION_MULTIPLIER)) :
1502 new OutlierFilter<>(parser.getInt(ParameterKey.AZ_EL_OUTLIER_REJECTION_STARTING_ITERATION),
1503 parser.getInt(ParameterKey.AZ_EL_OUTLIER_REJECTION_MULTIPLIER));
1504 }
1505
1506
1507
1508
1509
1510
1511 private OutlierFilter<PV> createPVOutliersManager(final KeyValueFileParser<ParameterKey> parser, boolean isDynamic) {
1512 needsBothOrNeither(parser,
1513 ParameterKey.PV_OUTLIER_REJECTION_MULTIPLIER,
1514 ParameterKey.PV_OUTLIER_REJECTION_STARTING_ITERATION);
1515 return isDynamic ?
1516 new DynamicOutlierFilter<>(parser.getInt(ParameterKey.PV_OUTLIER_REJECTION_STARTING_ITERATION),
1517 parser.getInt(ParameterKey.PV_OUTLIER_REJECTION_MULTIPLIER)) :
1518 new OutlierFilter<>(parser.getInt(ParameterKey.PV_OUTLIER_REJECTION_STARTING_ITERATION),
1519 parser.getInt(ParameterKey.PV_OUTLIER_REJECTION_MULTIPLIER));
1520 }
1521
1522
1523
1524
1525
1526
1527 private PVData createPVData(final KeyValueFileParser<ParameterKey> parser)
1528 throws NoSuchElementException {
1529 return new PVData(parser.getDouble(ParameterKey.PV_MEASUREMENTS_POSITION_SIGMA),
1530 parser.getDouble(ParameterKey.PV_MEASUREMENTS_VELOCITY_SIGMA));
1531 }
1532
1533
1534
1535
1536
1537
1538 private ObservableSatellite createObservableSatellite(final KeyValueFileParser<ParameterKey> parser)
1539 throws NoSuchElementException {
1540 final ObservableSatellite obsSat = new ObservableSatellite(0);
1541 final ParameterDriver clockOffsetDriver = obsSat.getClockOffsetDriver();
1542 if (parser.containsKey(ParameterKey.ON_BOARD_CLOCK_OFFSET)) {
1543 clockOffsetDriver.setReferenceValue(parser.getDouble(ParameterKey.ON_BOARD_CLOCK_OFFSET));
1544 clockOffsetDriver.setValue(parser.getDouble(ParameterKey.ON_BOARD_CLOCK_OFFSET));
1545 }
1546 if (parser.containsKey(ParameterKey.ON_BOARD_CLOCK_OFFSET_MIN)) {
1547 clockOffsetDriver.setMinValue(parser.getDouble(ParameterKey.ON_BOARD_CLOCK_OFFSET_MIN));
1548 }
1549 if (parser.containsKey(ParameterKey.ON_BOARD_CLOCK_OFFSET_MAX)) {
1550 clockOffsetDriver.setMaxValue(parser.getDouble(ParameterKey.ON_BOARD_CLOCK_OFFSET_MAX));
1551 }
1552 if (parser.containsKey(ParameterKey.ON_BOARD_CLOCK_OFFSET_ESTIMATED)) {
1553 clockOffsetDriver.setSelected(parser.getBoolean(ParameterKey.ON_BOARD_CLOCK_OFFSET_ESTIMATED));
1554 }
1555 return obsSat;
1556 }
1557
1558
1559
1560
1561
1562
1563
1564 private BatchLSEstimator createEstimator(final KeyValueFileParser<ParameterKey> parser,
1565 final IntegratedPropagatorBuilder propagatorBuilder)
1566 throws NoSuchElementException {
1567
1568 final boolean optimizerIsLevenbergMarquardt;
1569 if (!parser.containsKey(ParameterKey.ESTIMATOR_OPTIMIZATION_ENGINE)) {
1570 optimizerIsLevenbergMarquardt = true;
1571 } else {
1572 final String engine = parser.getString(ParameterKey.ESTIMATOR_OPTIMIZATION_ENGINE);
1573 optimizerIsLevenbergMarquardt = engine.toLowerCase().contains("levenberg");
1574 }
1575 final LeastSquaresOptimizer optimizer;
1576
1577 if (optimizerIsLevenbergMarquardt) {
1578
1579 final double initialStepBoundFactor;
1580 if (!parser.containsKey(ParameterKey.ESTIMATOR_LEVENBERG_MARQUARDT_INITIAL_STEP_BOUND_FACTOR)) {
1581 initialStepBoundFactor = 100.0;
1582 } else {
1583 initialStepBoundFactor = parser.getDouble(ParameterKey.ESTIMATOR_LEVENBERG_MARQUARDT_INITIAL_STEP_BOUND_FACTOR);
1584 }
1585
1586 optimizer = new LevenbergMarquardtOptimizer().withInitialStepBoundFactor(initialStepBoundFactor);
1587 } else {
1588
1589 optimizer = new GaussNewtonOptimizer(new QRDecomposer(1e-11), false);
1590 }
1591
1592 final double convergenceThreshold;
1593 if (!parser.containsKey(ParameterKey.ESTIMATOR_NORMALIZED_PARAMETERS_CONVERGENCE_THRESHOLD)) {
1594 convergenceThreshold = 1.0e-3;
1595 } else {
1596 convergenceThreshold = parser.getDouble(ParameterKey.ESTIMATOR_NORMALIZED_PARAMETERS_CONVERGENCE_THRESHOLD);
1597 }
1598 final int maxIterations;
1599 if (!parser.containsKey(ParameterKey.ESTIMATOR_MAX_ITERATIONS)) {
1600 maxIterations = 10;
1601 } else {
1602 maxIterations = parser.getInt(ParameterKey.ESTIMATOR_MAX_ITERATIONS);
1603 }
1604 final int maxEvaluations;
1605 if (!parser.containsKey(ParameterKey.ESTIMATOR_MAX_EVALUATIONS)) {
1606 maxEvaluations = 20;
1607 } else {
1608 maxEvaluations = parser.getInt(ParameterKey.ESTIMATOR_MAX_EVALUATIONS);
1609 }
1610
1611 final BatchLSEstimator estimator = new BatchLSEstimator(optimizer, propagatorBuilder);
1612 estimator.setParametersConvergenceThreshold(convergenceThreshold);
1613 estimator.setMaxIterations(maxIterations);
1614 estimator.setMaxEvaluations(maxEvaluations);
1615
1616 return estimator;
1617
1618 }
1619
1620
1621
1622
1623
1624
1625
1626
1627
1628
1629
1630
1631
1632
1633
1634
1635 private List<ObservedMeasurement<?>> readMeasurements(final NamedData nd,
1636 final Map<String, StationData> stations,
1637 final PVData pvData,
1638 final ObservableSatellite satellite,
1639 final Bias<Range> satRangeBias,
1640 final OnBoardAntennaRangeModifier satAntennaRangeModifier,
1641 final Weights weights,
1642 final OutlierFilter<Range> rangeOutliersManager,
1643 final OutlierFilter<RangeRate> rangeRateOutliersManager,
1644 final OutlierFilter<AngularAzEl> azElOutliersManager,
1645 final OutlierFilter<PV> pvOutliersManager)
1646 throws IOException {
1647
1648 final List<ObservedMeasurement<?>> measurements = new ArrayList<ObservedMeasurement<?>>();
1649 try (InputStream is = nd.getStreamOpener().openStream();
1650 InputStreamReader isr = new InputStreamReader(is, StandardCharsets.UTF_8);
1651 BufferedReader br = new BufferedReader(isr)) {
1652 int lineNumber = 0;
1653 for (String line = br.readLine(); line != null; line = br.readLine()) {
1654 ++lineNumber;
1655 line = line.trim();
1656 if (line.length() > 0 && !line.startsWith("#")) {
1657 final String[] fields = line.split("\\s+");
1658 if (fields.length < 2) {
1659 throw new OrekitException(OrekitMessages.UNABLE_TO_PARSE_LINE_IN_FILE,
1660 lineNumber, nd.getName(), line);
1661 }
1662 switch (fields[1]) {
1663 case "RANGE" :
1664 final Range range = new RangeParser().parseFields(fields, stations, pvData, satellite,
1665 satRangeBias, weights,
1666 line, lineNumber, nd.getName());
1667 if (satAntennaRangeModifier != null) {
1668 range.addModifier(satAntennaRangeModifier);
1669 }
1670 if (rangeOutliersManager != null) {
1671 range.addModifier(rangeOutliersManager);
1672 }
1673 addIfNonZeroWeight(range, measurements);
1674 break;
1675 case "RANGE_RATE" :
1676 final RangeRate rangeRate = new RangeRateParser().parseFields(fields, stations, pvData, satellite,
1677 satRangeBias, weights,
1678 line, lineNumber, nd.getName());
1679 if (rangeRateOutliersManager != null) {
1680 rangeRate.addModifier(rangeRateOutliersManager);
1681 }
1682 addIfNonZeroWeight(rangeRate, measurements);
1683 break;
1684 case "AZ_EL" :
1685 final AngularAzEl angular = new AzElParser().parseFields(fields, stations, pvData, satellite,
1686 satRangeBias, weights,
1687 line, lineNumber, nd.getName());
1688 if (azElOutliersManager != null) {
1689 angular.addModifier(azElOutliersManager);
1690 }
1691 addIfNonZeroWeight(angular, measurements);
1692 break;
1693 case "PV" :
1694 final PV pv = new PVParser().parseFields(fields, stations, pvData, satellite,
1695 satRangeBias, weights,
1696 line, lineNumber, nd.getName());
1697 if (pvOutliersManager != null) {
1698 pv.addModifier(pvOutliersManager);
1699 }
1700 addIfNonZeroWeight(pv, measurements);
1701 break;
1702 default :
1703 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
1704 "unknown measurement type " + fields[1] +
1705 " at line " + lineNumber +
1706 " in file " + nd.getName() +
1707 "\n" + line);
1708 }
1709 }
1710 }
1711 }
1712
1713 if (measurements.isEmpty()) {
1714 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
1715 "not measurements read from file " + nd.getName());
1716 }
1717
1718 return measurements;
1719
1720 }
1721
1722
1723
1724
1725
1726
1727
1728
1729
1730
1731
1732
1733
1734
1735 private List<ObservedMeasurement<?>> readRinex(final NamedData nd, final String satId,
1736 final Map<String, StationData> stations,
1737 final ObservableSatellite satellite,
1738 final Bias<Range> satRangeBias,
1739 final OnBoardAntennaRangeModifier satAntennaRangeModifier,
1740 final Weights weights,
1741 final OutlierFilter<Range> rangeOutliersManager,
1742 final OutlierFilter<RangeRate> rangeRateOutliersManager)
1743 throws IOException {
1744 final String notConfigured = " not configured";
1745 final List<ObservedMeasurement<?>> measurements = new ArrayList<ObservedMeasurement<?>>();
1746 final SatelliteSystem system = SatelliteSystem.parseSatelliteSystem(satId);
1747 final int prnNumber;
1748 switch (system) {
1749 case GPS:
1750 case GLONASS:
1751 case GALILEO:
1752 prnNumber = Integer.parseInt(satId.substring(1));
1753 break;
1754 case SBAS:
1755 prnNumber = Integer.parseInt(satId.substring(1)) + 100;
1756 break;
1757 default:
1758 prnNumber = -1;
1759 }
1760 final Ionommon/Iono.html#Iono">Iono iono = new Iono(false);
1761 final RinexLoader loader = new RinexLoader(nd.getStreamOpener().openStream(), nd.getName());
1762 for (final ObservationDataSet observationDataSet : loader.getObservationDataSets()) {
1763 if (observationDataSet.getSatelliteSystem() == system &&
1764 observationDataSet.getPrnNumber() == prnNumber) {
1765 for (final ObservationData od : observationDataSet.getObservationData()) {
1766 if (!Double.isNaN(od.getValue())) {
1767 if (od.getObservationType().getMeasurementType() == MeasurementType.PSEUDO_RANGE) {
1768
1769 final String stationName = observationDataSet.getHeader().getMarkerName() + "/" + od.getObservationType();
1770 final StationData stationData = stations.get(stationName);
1771 if (stationData == null) {
1772 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
1773 stationName + notConfigured);
1774 }
1775 final Range range = new Range(stationData.getStation(), false, observationDataSet.getDate(),
1776 od.getValue(), stationData.getRangeSigma(),
1777 weights.getRangeBaseWeight(), satellite);
1778 range.addModifier(iono.getRangeModifier(od.getObservationType().getFrequency(system),
1779 observationDataSet.getDate()));
1780 if (satAntennaRangeModifier != null) {
1781 range.addModifier(satAntennaRangeModifier);
1782 }
1783 if (stationData.getRangeBias() != null) {
1784 range.addModifier(stationData.getRangeBias());
1785 }
1786 if (satRangeBias != null) {
1787 range.addModifier(satRangeBias);
1788 }
1789 if (stationData.getRangeTroposphericCorrection() != null) {
1790 range.addModifier(stationData.getRangeTroposphericCorrection());
1791 }
1792 addIfNonZeroWeight(range, measurements);
1793
1794 } else if (od.getObservationType().getMeasurementType() == MeasurementType.DOPPLER) {
1795
1796 final String stationName = observationDataSet.getHeader().getMarkerName() + "/" + od.getObservationType();
1797 final StationData stationData = stations.get(stationName);
1798 if (stationData == null) {
1799 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
1800 stationName + notConfigured);
1801 }
1802 final RangeRate rangeRate = new RangeRate(stationData.getStation(), observationDataSet.getDate(),
1803 od.getValue(), stationData.getRangeRateSigma(),
1804 weights.getRangeRateBaseWeight(), false, satellite);
1805 rangeRate.addModifier(iono.getRangeRateModifier(od.getObservationType().getFrequency(system),
1806 observationDataSet.getDate()));
1807 if (stationData.getRangeRateBias() != null) {
1808 rangeRate.addModifier(stationData.getRangeRateBias());
1809 }
1810 addIfNonZeroWeight(rangeRate, measurements);
1811 }
1812 }
1813 }
1814 }
1815 }
1816
1817 return measurements;
1818
1819 }
1820
1821
1822
1823
1824
1825
1826 private List<ObservedMeasurement<?>> multiplexMeasurements(final List<ObservedMeasurement<?>> independentMeasurements,
1827 final double tol) {
1828 final List<ObservedMeasurement<?>> multiplexed = new ArrayList<>();
1829 independentMeasurements.sort(new ChronologicalComparator());
1830 List<ObservedMeasurement<?>> clump = new ArrayList<>();
1831 for (final ObservedMeasurement<?> measurement : independentMeasurements) {
1832 if (!clump.isEmpty() && measurement.getDate().durationFrom(clump.get(0).getDate()) > tol) {
1833
1834
1835 if (clump.size() == 1) {
1836 multiplexed.add(clump.get(0));
1837 } else {
1838 multiplexed.add(new MultiplexedMeasurement(clump));
1839 }
1840
1841
1842 clump = new ArrayList<>();
1843
1844 }
1845 clump.add(measurement);
1846 }
1847
1848 if (clump.size() == 1) {
1849 multiplexed.add(clump.get(0));
1850 } else {
1851 multiplexed.add(new MultiplexedMeasurement(clump));
1852 }
1853 return multiplexed;
1854 }
1855
1856
1857
1858
1859 protected void sortParametersChanges(List<? extends ParameterDriver> parameters) {
1860
1861
1862 Collections.sort(parameters, new Comparator<ParameterDriver>() {
1863
1864 @Override
1865 public int compare(final ParameterDriver pd1, final ParameterDriver pd2) {
1866 return pd1.getName().compareTo(pd2.getName());
1867 }
1868
1869 });
1870 }
1871
1872
1873
1874
1875
1876 private static void addIfNonZeroWeight(final ObservedMeasurement<?> measurement, final List<ObservedMeasurement<?>> measurements) {
1877 double sum = 0;
1878 for (double w : measurement.getBaseWeight()) {
1879 sum += FastMath.abs(w);
1880 }
1881 if (sum > Precision.SAFE_MIN) {
1882
1883 measurements.add(measurement);
1884 }
1885 }
1886
1887
1888
1889
1890
1891
1892 private void needsBothOrNeither(final KeyValueFileParser<ParameterKey> parser,
1893 final ParameterKeymeterKey.html#ParameterKey">ParameterKey key1, final ParameterKey key2) {
1894 if (parser.containsKey(key1) != parser.containsKey(key2)) {
1895 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
1896 key1.toString().toLowerCase().replace('_', '.') +
1897 " and " +
1898 key2.toString().toLowerCase().replace('_', '.') +
1899 " must be both present or both absent");
1900 }
1901 }
1902
1903
1904
1905
1906
1907
1908
1909 private void displayParametersChanges(final PrintStream out, final String header, final boolean sort,
1910 final int length, final ParameterDriversList parameters) {
1911
1912 List<ParameterDriver> list = new ArrayList<ParameterDriver>(parameters.getDrivers());
1913 if (sort) {
1914
1915 Collections.sort(list, new Comparator<ParameterDriver>() {
1916
1917 @Override
1918 public int compare(final ParameterDriver pd1, final ParameterDriver pd2) {
1919 return pd1.getName().compareTo(pd2.getName());
1920 }
1921
1922 });
1923 }
1924
1925 out.println(header);
1926 int index = 0;
1927 for (final ParameterDriver parameter : list) {
1928 if (parameter.isSelected()) {
1929 final double factor;
1930 if (parameter.getName().endsWith("/az bias") || parameter.getName().endsWith("/el bias")) {
1931 factor = FastMath.toDegrees(1.0);
1932 } else {
1933 factor = 1.0;
1934 }
1935 final double initial = parameter.getReferenceValue();
1936 final double value = parameter.getValue();
1937 out.format(Locale.US, " %2d %s", ++index, parameter.getName());
1938 for (int i = parameter.getName().length(); i < length; ++i) {
1939 out.format(Locale.US, " ");
1940 }
1941 out.format(Locale.US, " %+.12f (final value: % .12f)%n",
1942 factor * (value - initial), factor * value);
1943 }
1944 }
1945
1946 }
1947
1948
1949
1950 private void displayFinalCovariances(final PrintStream logStream, final KalmanEstimator kalman) {
1951
1952
1953
1954
1955
1956
1957
1958
1959
1960
1961
1962
1963
1964
1965
1966
1967
1968
1969
1970
1971
1972
1973
1974 final RealMatrix P = kalman.getPhysicalEstimatedCovarianceMatrix();
1975 final String[] paramNames = new String[P.getRowDimension()];
1976 int index = 0;
1977 int paramSize = 0;
1978 for (final ParameterDriver driver : kalman.getOrbitalParametersDrivers(true).getDrivers()) {
1979 paramNames[index++] = driver.getName();
1980 paramSize = FastMath.max(paramSize, driver.getName().length());
1981 }
1982 for (final ParameterDriver driver : kalman.getPropagationParametersDrivers(true).getDrivers()) {
1983 paramNames[index++] = driver.getName();
1984 paramSize = FastMath.max(paramSize, driver.getName().length());
1985 }
1986 for (final ParameterDriver driver : kalman.getEstimatedMeasurementsParameters().getDrivers()) {
1987 paramNames[index++] = driver.getName();
1988 paramSize = FastMath.max(paramSize, driver.getName().length());
1989 }
1990 if (paramSize < 20) {
1991 paramSize = 20;
1992 }
1993
1994
1995 logStream.format("\n%s\n", "Kalman Final Covariances:");
1996
1997
1998 logStream.format(Locale.US, "\tDate: %-23s UTC\n",
1999 kalman.getCurrentDate().toString(TimeScalesFactory.getUTC()));
2000
2001
2002 String strFormat = String.format("%%%2ds ", paramSize);
2003 logStream.format(strFormat, "Covariances:");
2004 for (int i = 0; i < P.getRowDimension(); i++) {
2005 logStream.format(Locale.US, strFormat, paramNames[i]);
2006 }
2007 logStream.println("");
2008 String numFormat = String.format("%%%2d.6f ", paramSize);
2009 for (int i = 0; i < P.getRowDimension(); i++) {
2010 logStream.format(Locale.US, strFormat, paramNames[i]);
2011 for (int j = 0; j <= i; j++) {
2012 logStream.format(Locale.US, numFormat, P.getEntry(i, j));
2013 }
2014 logStream.println("");
2015 }
2016
2017
2018 final double[] sigmas = new double[P.getRowDimension()];
2019 for (int i = 0; i < P.getRowDimension(); i++) {
2020 sigmas[i] = FastMath.sqrt(P.getEntry(i, i));
2021 }
2022
2023 logStream.format("\n" + strFormat, "Corr coef:");
2024 for (int i = 0; i < P.getRowDimension(); i++) {
2025 logStream.format(Locale.US, strFormat, paramNames[i]);
2026 }
2027 logStream.println("");
2028 for (int i = 0; i < P.getRowDimension(); i++) {
2029 logStream.format(Locale.US, strFormat, paramNames[i]);
2030 for (int j = 0; j <= i; j++) {
2031 logStream.format(Locale.US, numFormat, P.getEntry(i, j)/(sigmas[i]*sigmas[j]));
2032 }
2033 logStream.println("");
2034 }
2035
2036
2037 logStream.format("\n" + strFormat + "\n", "Sigmas: ");
2038 for (int i = 0; i < P.getRowDimension(); i++) {
2039 logStream.format(Locale.US, strFormat + numFormat + "\n", paramNames[i], sigmas[i]);
2040 }
2041 logStream.println("");
2042 }
2043
2044
2045
2046 private void logEvaluation(EstimatedMeasurement<?> evaluation,
2047 EvaluationLogger<Range> rangeLog,
2048 EvaluationLogger<RangeRate> rangeRateLog,
2049 EvaluationLogger<AngularAzEl> azimuthLog,
2050 EvaluationLogger<AngularAzEl> elevationLog,
2051 EvaluationLogger<PV> positionLog,
2052 EvaluationLogger<PV> velocityLog) {
2053 if (evaluation.getObservedMeasurement() instanceof Range) {
2054 @SuppressWarnings("unchecked")
2055 final EstimatedMeasurement<Range> ev = (EstimatedMeasurement<Range>) evaluation;
2056 if (rangeLog != null) {
2057 rangeLog.log(ev);
2058 }
2059 } else if (evaluation.getObservedMeasurement() instanceof RangeRate) {
2060 @SuppressWarnings("unchecked")
2061 final EstimatedMeasurement<RangeRate> ev = (EstimatedMeasurement<RangeRate>) evaluation;
2062 if (rangeRateLog != null) {
2063 rangeRateLog.log(ev);
2064 }
2065 } else if (evaluation.getObservedMeasurement() instanceof AngularAzEl) {
2066 @SuppressWarnings("unchecked")
2067 final EstimatedMeasurement<AngularAzEl> ev = (EstimatedMeasurement<AngularAzEl>) evaluation;
2068 if (azimuthLog != null) {
2069 azimuthLog.log(ev);
2070 }
2071 if (elevationLog != null) {
2072 elevationLog.log(ev);
2073 }
2074 } else if (evaluation.getObservedMeasurement() instanceof PV) {
2075 @SuppressWarnings("unchecked")
2076 final EstimatedMeasurement<PV> ev = (EstimatedMeasurement<PV>) evaluation;
2077 if (positionLog != null) {
2078 positionLog.log(ev);
2079 }
2080 if (velocityLog != null) {
2081 velocityLog.log(ev);
2082 }
2083 } else if (evaluation.getObservedMeasurement() instanceof MultiplexedMeasurement) {
2084 for (final EstimatedMeasurement<?> em : ((MultiplexedMeasurement) evaluation.getObservedMeasurement()).getEstimatedMeasurements()) {
2085 logEvaluation(em, rangeLog, rangeRateLog, azimuthLog, elevationLog, positionLog, velocityLog);
2086 }
2087 }
2088 }
2089
2090 }