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.PrintStream;
24 import java.io.Reader;
25 import java.nio.file.Files;
26 import java.util.ArrayList;
27 import java.util.Arrays;
28 import java.util.Comparator;
29 import java.util.HashMap;
30 import java.util.List;
31 import java.util.Locale;
32 import java.util.Map;
33 import java.util.NoSuchElementException;
34 import java.util.regex.Pattern;
35
36 import org.hipparchus.exception.LocalizedCoreFormats;
37 import org.hipparchus.geometry.euclidean.threed.Vector3D;
38 import org.hipparchus.linear.MatrixUtils;
39 import org.hipparchus.linear.QRDecomposer;
40 import org.hipparchus.linear.RealMatrix;
41 import org.hipparchus.optim.nonlinear.vector.leastsquares.GaussNewtonOptimizer;
42 import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer;
43 import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer.Optimum;
44 import org.hipparchus.optim.nonlinear.vector.leastsquares.LevenbergMarquardtOptimizer;
45 import org.hipparchus.optim.nonlinear.vector.leastsquares.SequentialGaussNewtonOptimizer;
46 import org.hipparchus.util.FastMath;
47 import org.hipparchus.util.MerweUnscentedTransform;
48 import org.hipparchus.util.Precision;
49 import org.junit.jupiter.api.Assertions;
50 import org.orekit.KeyValueFileParser;
51 import org.orekit.attitudes.AttitudeProvider;
52 import org.orekit.bodies.CelestialBody;
53 import org.orekit.bodies.CelestialBodyFactory;
54 import org.orekit.bodies.GeodeticPoint;
55 import org.orekit.bodies.OneAxisEllipsoid;
56 import org.orekit.data.DataFilter;
57 import org.orekit.data.DataSource;
58 import org.orekit.data.GzipFilter;
59 import org.orekit.data.UnixCompressFilter;
60 import org.orekit.errors.OrekitException;
61 import org.orekit.errors.OrekitMessages;
62 import org.orekit.estimation.leastsquares.BatchLSEstimator;
63 import org.orekit.estimation.leastsquares.SequentialBatchLSEstimator;
64 import org.orekit.estimation.measurements.AngularAzEl;
65 import org.orekit.estimation.measurements.EstimatedMeasurement;
66 import org.orekit.estimation.measurements.GroundStation;
67 import org.orekit.estimation.measurements.MultiplexedMeasurement;
68 import org.orekit.estimation.measurements.ObservableSatellite;
69 import org.orekit.estimation.measurements.ObservedMeasurement;
70 import org.orekit.estimation.measurements.PV;
71 import org.orekit.estimation.measurements.Position;
72 import org.orekit.estimation.measurements.Range;
73 import org.orekit.estimation.measurements.RangeRate;
74 import org.orekit.estimation.measurements.modifiers.AngularRadioRefractionModifier;
75 import org.orekit.estimation.measurements.modifiers.Bias;
76 import org.orekit.estimation.measurements.modifiers.DynamicOutlierFilter;
77 import org.orekit.estimation.measurements.modifiers.OutlierFilter;
78 import org.orekit.estimation.measurements.modifiers.PhaseCentersRangeModifier;
79 import org.orekit.estimation.measurements.modifiers.RangeIonosphericDelayModifier;
80 import org.orekit.estimation.measurements.modifiers.RangeRateIonosphericDelayModifier;
81 import org.orekit.estimation.measurements.modifiers.RangeTroposphericDelayModifier;
82 import org.orekit.estimation.measurements.modifiers.ShapiroRangeModifier;
83 import org.orekit.estimation.sequential.*;
84 import org.orekit.files.ilrs.CPF;
85 import org.orekit.files.ilrs.CPF.CPFCoordinate;
86 import org.orekit.files.ilrs.CPF.CPFEphemeris;
87 import org.orekit.files.ilrs.CPFParser;
88 import org.orekit.files.ilrs.CRD;
89 import org.orekit.files.ilrs.CRD.CRDDataBlock;
90 import org.orekit.files.ilrs.CRD.Meteo;
91 import org.orekit.files.ilrs.CRD.MeteorologicalMeasurement;
92 import org.orekit.files.ilrs.CRD.RangeMeasurement;
93 import org.orekit.files.ilrs.CRDHeader;
94 import org.orekit.files.ilrs.CRDHeader.RangeType;
95 import org.orekit.files.ilrs.CRDParser;
96 import org.orekit.files.rinex.HatanakaCompressFilter;
97 import org.orekit.files.rinex.observation.ObservationData;
98 import org.orekit.files.rinex.observation.ObservationDataSet;
99 import org.orekit.files.rinex.observation.RinexObservation;
100 import org.orekit.files.rinex.observation.RinexObservationParser;
101 import org.orekit.files.sinex.Sinex;
102 import org.orekit.files.sinex.SinexParser;
103 import org.orekit.files.sinex.Station;
104 import org.orekit.forces.drag.DragSensitive;
105 import org.orekit.forces.drag.IsotropicDrag;
106 import org.orekit.forces.gravity.potential.GravityFieldFactory;
107 import org.orekit.forces.radiation.IsotropicRadiationSingleCoefficient;
108 import org.orekit.forces.radiation.RadiationSensitive;
109 import org.orekit.frames.EOPHistory;
110 import org.orekit.frames.Frame;
111 import org.orekit.frames.FramesFactory;
112 import org.orekit.frames.TopocentricFrame;
113 import org.orekit.gnss.MeasurementType;
114 import org.orekit.gnss.SatelliteSystem;
115 import org.orekit.gnss.antenna.FrequencyPattern;
116 import org.orekit.models.AtmosphericRefractionModel;
117 import org.orekit.models.earth.ITURP834AtmosphericRefraction;
118 import org.orekit.models.earth.Geoid;
119 import org.orekit.models.earth.ReferenceEllipsoid;
120 import org.orekit.models.earth.atmosphere.Atmosphere;
121 import org.orekit.models.earth.atmosphere.DTM2000;
122 import org.orekit.models.earth.atmosphere.data.MarshallSolarActivityFutureEstimation;
123 import org.orekit.models.earth.displacement.OceanLoading;
124 import org.orekit.models.earth.displacement.OceanLoadingCoefficientsBLQFactory;
125 import org.orekit.models.earth.displacement.StationDisplacement;
126 import org.orekit.models.earth.displacement.TidalDisplacement;
127 import org.orekit.models.earth.ionosphere.EstimatedIonosphericModel;
128 import org.orekit.models.earth.ionosphere.IonosphericMappingFunction;
129 import org.orekit.models.earth.ionosphere.IonosphericModel;
130 import org.orekit.models.earth.ionosphere.KlobucharIonoCoefficientsLoader;
131 import org.orekit.models.earth.ionosphere.KlobucharIonoModel;
132 import org.orekit.models.earth.ionosphere.SingleLayerModelMappingFunction;
133 import org.orekit.models.earth.troposphere.EstimatedModel;
134 import org.orekit.models.earth.troposphere.GlobalMappingFunctionModel;
135 import org.orekit.models.earth.troposphere.MendesPavlisModel;
136 import org.orekit.models.earth.troposphere.ModifiedSaastamoinenModel;
137 import org.orekit.models.earth.troposphere.NiellMappingFunctionModel;
138 import org.orekit.models.earth.troposphere.TimeSpanEstimatedModel;
139 import org.orekit.models.earth.troposphere.TroposphereMappingFunction;
140 import org.orekit.models.earth.troposphere.TroposphericModel;
141 import org.orekit.models.earth.troposphere.TroposphericModelUtils;
142 import org.orekit.models.earth.weather.ConstantPressureTemperatureHumidityProvider;
143 import org.orekit.models.earth.weather.GlobalPressureTemperature;
144 import org.orekit.models.earth.weather.PressureTemperature;
145 import org.orekit.models.earth.weather.PressureTemperatureHumidity;
146 import org.orekit.models.earth.weather.PressureTemperatureHumidityProvider;
147 import org.orekit.models.earth.weather.water.CIPM2007;
148 import org.orekit.orbits.CartesianOrbit;
149 import org.orekit.orbits.CircularOrbit;
150 import org.orekit.orbits.EquinoctialOrbit;
151 import org.orekit.orbits.KeplerianOrbit;
152 import org.orekit.orbits.Orbit;
153 import org.orekit.orbits.OrbitType;
154 import org.orekit.orbits.PositionAngleType;
155 import org.orekit.propagation.Propagator;
156 import org.orekit.propagation.SpacecraftState;
157 import org.orekit.propagation.analytical.tle.TLE;
158 import org.orekit.propagation.analytical.tle.TLEPropagator;
159 import org.orekit.propagation.conversion.DormandPrince853IntegratorBuilder;
160 import org.orekit.propagation.conversion.ODEIntegratorBuilder;
161 import org.orekit.propagation.conversion.PropagatorBuilder;
162 import org.orekit.time.AbsoluteDate;
163 import org.orekit.time.ChronologicalComparator;
164 import org.orekit.time.TimeScale;
165 import org.orekit.time.TimeScalesFactory;
166 import org.orekit.utils.Constants;
167 import org.orekit.utils.IERSConventions;
168 import org.orekit.utils.PVCoordinates;
169 import org.orekit.utils.ParameterDriver;
170 import org.orekit.utils.ParameterDriversList;
171 import org.orekit.utils.ParameterDriversList.DelegatingDriver;
172 import org.orekit.utils.TimeSpanMap.Span;
173 import org.orekit.utils.units.Unit;
174
175
176
177
178
179
180
181 public abstract class AbstractOrbitDetermination<T extends PropagatorBuilder> {
182
183
184 private static final String RANGE_BIAS_SUFFIX = "/range bias";
185
186
187 private static final String RANGE_RATE_BIAS_SUFFIX = "/range rate bias";
188
189
190 private static final String AZIMUTH_BIAS_SUFFIX = "/az bias";
191
192
193 private static final String ELEVATION_BIAS_SUFFIX = "/el bias";
194
195
196 private static final String CPF_MANDATORY_KEY = "cpf";
197
198
199 private boolean useRangeMeasurements;
200
201
202 private boolean useRangeRateMeasurements;
203
204
205
206
207
208 protected abstract void createGravityField(KeyValueFileParser<ParameterKey> parser)
209 throws NoSuchElementException;
210
211
212
213
214 protected abstract double getMu();
215
216
217
218
219
220
221
222
223
224
225
226
227
228 protected abstract T createPropagatorBuilder(Orbit referenceOrbit,
229 ODEIntegratorBuilder builder,
230 double positionScale);
231
232
233
234
235
236 protected abstract void setMass(T propagatorBuilder, double mass);
237
238
239
240
241
242
243 protected abstract List<ParameterDriver> setGravity(T propagatorBuilder, OneAxisEllipsoid body);
244
245
246
247
248
249
250
251
252
253 protected abstract List<ParameterDriver> setOceanTides(T propagatorBuilder, IERSConventions conventions,
254 OneAxisEllipsoid body, int degree, int order);
255
256
257
258
259
260
261
262
263 protected abstract List<ParameterDriver> setSolidTides(T propagatorBuilder, IERSConventions conventions,
264 OneAxisEllipsoid body, CelestialBody[] solidTidesBodies);
265
266
267
268
269
270
271 protected abstract List<ParameterDriver> setThirdBody(T propagatorBuilder, CelestialBody thirdBody);
272
273
274
275
276
277
278
279 protected abstract List<ParameterDriver> setDrag(T propagatorBuilder, Atmosphere atmosphere, DragSensitive spacecraft);
280
281
282
283
284
285
286
287
288 protected abstract List<ParameterDriver> setSolarRadiationPressure(T propagatorBuilder, CelestialBody sun,
289 OneAxisEllipsoid body, RadiationSensitive spacecraft);
290
291
292
293
294
295
296
297
298
299 protected abstract List<ParameterDriver> setAlbedoInfrared(T propagatorBuilder, CelestialBody sun,
300 double equatorialRadius, double angularResolution,
301 RadiationSensitive spacecraft);
302
303
304
305
306
307 protected abstract List<ParameterDriver> setRelativity(T propagatorBuilder);
308
309
310
311
312
313
314
315
316 protected abstract List<ParameterDriver> setPolynomialAcceleration(T propagatorBuilder, String name,
317 Vector3D direction, int degree);
318
319
320
321
322
323 protected abstract void setAttitudeProvider(T propagatorBuilder, AttitudeProvider attitudeProvider);
324
325
326
327
328
329
330 protected ResultBatchLeastSquares runBLS(final File input, final boolean print) throws IOException {
331
332
333 final KeyValueFileParser<ParameterKey> parser = new KeyValueFileParser<>(ParameterKey.class);
334 try (FileInputStream fis = new FileInputStream(input)) {
335 parser.parseInput(input.getAbsolutePath(), fis);
336 }
337
338 final RangeLog rangeLog = new RangeLog();
339 final RangeRateLog rangeRateLog = new RangeRateLog();
340 final AzimuthLog azimuthLog = new AzimuthLog();
341 final ElevationLog elevationLog = new ElevationLog();
342 final PositionOnlyLog positionOnlyLog = new PositionOnlyLog();
343 final PositionLog positionLog = new PositionLog();
344 final VelocityLog velocityLog = new VelocityLog();
345
346
347 createGravityField(parser);
348
349
350 final Orbit initialGuess = createOrbit(parser, getMu());
351
352
353 final IERSConventions conventions;
354 if (!parser.containsKey(ParameterKey.IERS_CONVENTIONS)) {
355 conventions = IERSConventions.IERS_2010;
356 } else {
357 conventions = IERSConventions.valueOf("IERS_" + parser.getInt(ParameterKey.IERS_CONVENTIONS));
358 }
359
360
361 final OneAxisEllipsoid body = createBody(parser);
362
363
364 final T propagatorBuilder = configurePropagatorBuilder(parser, conventions, body, initialGuess);
365
366
367 final BatchLSEstimator estimator = createEstimator(parser, propagatorBuilder);
368
369
370
371
372 final Sinex stationPositionData = readSinexFile(input, parser, ParameterKey.SINEX_POSITION_FILE);
373 final Sinex stationEccData = readSinexFile(input, parser, ParameterKey.SINEX_ECC_FILE);
374
375
376 useRangeMeasurements = parser.getBoolean(ParameterKey.USE_RANGE_MEASUREMENTS);
377 useRangeRateMeasurements = parser.getBoolean(ParameterKey.USE_RANGE_RATE_MEASUREMENTS);
378
379 final Map<String, StationData> stations = createStationsData(parser, initialGuess.getDate(),
380 stationPositionData, stationEccData, conventions, body);
381 final PVData pvData = createPVData(parser);
382 final ObservableSatellite satellite = createObservableSatellite(parser);
383 final Bias<Range> satRangeBias = createSatRangeBias(parser);
384 final PhaseCentersRangeModifier satAntennaRangeModifier = createSatAntennaRangeModifier(parser);
385 final ShapiroRangeModifier shapiroRangeModifier = createShapiroRangeModifier(parser);
386 final Weights weights = createWeights(parser);
387 final OutlierFilter<Range> rangeOutliersManager = createRangeOutliersManager(parser, false);
388 final OutlierFilter<RangeRate> rangeRateOutliersManager = createRangeRateOutliersManager(parser, false);
389 final OutlierFilter<AngularAzEl> azElOutliersManager = createAzElOutliersManager(parser, false);
390 final OutlierFilter<PV> pvOutliersManager = createPVOutliersManager(parser, false);
391
392
393 final List<ObservedMeasurement<?>> independentMeasurements = new ArrayList<>();
394 for (final String fileName : parser.getStringsList(ParameterKey.MEASUREMENTS_FILES, ',')) {
395
396
397 DataSource nd = new DataSource(fileName, () -> Files.newInputStream(new File(input.getParentFile(), fileName).toPath()));
398 for (final DataFilter filter : Arrays.asList(new GzipFilter(),
399 new UnixCompressFilter(),
400 new HatanakaCompressFilter())) {
401 nd = filter.filter(nd);
402 }
403
404 if (Pattern.matches(RinexObservationParser.DEFAULT_RINEX_2_NAMES, nd.getName()) ||
405 Pattern.matches(RinexObservationParser.DEFAULT_RINEX_3_NAMES, nd.getName())) {
406
407 independentMeasurements.addAll(readRinex(nd,
408 parser.getString(ParameterKey.SATELLITE_ID_IN_RINEX_FILES),
409 stations, satellite, satRangeBias, satAntennaRangeModifier, weights,
410 rangeOutliersManager, rangeRateOutliersManager, shapiroRangeModifier));
411 } else if (Pattern.matches(CRDParser.DEFAULT_CRD_SUPPORTED_NAMES, nd.getName())) {
412
413 independentMeasurements.addAll(readCrd(nd, stations, parser, satellite, satRangeBias,
414 weights, rangeOutliersManager, shapiroRangeModifier));
415 } else if (fileName.contains(CPF_MANDATORY_KEY)) {
416
417 independentMeasurements.addAll(readCpf(nd, satellite, initialGuess));
418 } else {
419
420 independentMeasurements.addAll(readMeasurements(nd,
421 stations, pvData, satellite,
422 satRangeBias, satAntennaRangeModifier, weights,
423 rangeOutliersManager,
424 rangeRateOutliersManager,
425 azElOutliersManager,
426 pvOutliersManager));
427 }
428
429 }
430 final List<ObservedMeasurement<?>> multiplexed = multiplexMeasurements(independentMeasurements, 1.0e-9);
431 multiplexed.forEach(estimator::addMeasurement);
432
433
434 if (print) {
435 final String header = "iteration evaluations ΔP(m) ΔV(m/s) RMS nb Range nb Range-rate nb Angular nb Position nb PV%n";
436 estimator.setObserver(new BatchLeastSquaresObserver(initialGuess, estimator, header, print));
437 }
438 final Orbit estimated = estimator.estimate()[0].getInitialState().getOrbit();
439
440
441 for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry : estimator.getLastEstimations().entrySet()) {
442 logEvaluation(entry.getValue(),
443 rangeLog, rangeRateLog, azimuthLog, elevationLog, positionOnlyLog, positionLog, velocityLog);
444 }
445
446 final ParameterDriversList propagatorParameters = estimator.getPropagatorParametersDrivers(true);
447 final ParameterDriversList measurementsParameters = estimator.getMeasurementsParametersDrivers(true);
448 return new ResultBatchLeastSquares(propagatorParameters, measurementsParameters,
449 estimator.getIterationsCount(), estimator.getEvaluationsCount(), estimated.getPVCoordinates(),
450 rangeLog.createStatisticsSummary(), rangeRateLog.createStatisticsSummary(),
451 azimuthLog.createStatisticsSummary(), elevationLog.createStatisticsSummary(),
452 positionLog.createStatisticsSummary(), velocityLog.createStatisticsSummary(),
453 estimator.getPhysicalCovariances(1.0e-10));
454
455 }
456
457
458
459
460
461 protected ResultSequentialBatchLeastSquares runSequentialBLS(final File inputModel, final boolean print) throws IOException {
462
463
464 final KeyValueFileParser<ParameterKey> parser = new KeyValueFileParser<>(ParameterKey.class);
465 try (FileInputStream fis = new FileInputStream(inputModel)) {
466 parser.parseInput(inputModel.getAbsolutePath(), fis);
467 }
468
469 final RangeLog rangeLog = new RangeLog();
470 final RangeRateLog rangeRateLog = new RangeRateLog();
471 final AzimuthLog azimuthLog = new AzimuthLog();
472 final ElevationLog elevationLog = new ElevationLog();
473 final PositionOnlyLog positionOnlyLog = new PositionOnlyLog();
474 final PositionLog positionLog = new PositionLog();
475 final VelocityLog velocityLog = new VelocityLog();
476
477
478 createGravityField(parser);
479
480
481 final Orbit initialGuess = createOrbit(parser, getMu());
482
483
484 final IERSConventions conventions;
485 if (!parser.containsKey(ParameterKey.IERS_CONVENTIONS)) {
486 conventions = IERSConventions.IERS_2010;
487 } else {
488 conventions = IERSConventions.valueOf("IERS_" + parser.getInt(ParameterKey.IERS_CONVENTIONS));
489 }
490
491
492 final OneAxisEllipsoid body = createBody(parser);
493
494
495 final T propagatorBuilder = configurePropagatorBuilder(parser, conventions, body, initialGuess);
496
497
498 BatchLSEstimator estimator = createEstimator(parser, propagatorBuilder);
499
500 final ObservableSatellite satellite = createObservableSatellite(parser);
501
502
503 List<ObservedMeasurement<?>> independentMeasurements = new ArrayList<>();
504 for (final String fileName : parser.getStringsList(ParameterKey.MEASUREMENTS_FILES, ',')) {
505
506
507 DataSource nd = new DataSource(fileName, () -> Files.newInputStream(new File(inputModel.getParentFile(), fileName).toPath()));
508 for (final DataFilter filter : Arrays.asList(new GzipFilter(),
509 new UnixCompressFilter(),
510 new HatanakaCompressFilter())) {
511 nd = filter.filter(nd);
512 }
513
514 if (fileName.contains(CPF_MANDATORY_KEY)) {
515
516 independentMeasurements.addAll(readCpf(nd, satellite, initialGuess));
517 }
518
519 }
520
521
522 List<ObservedMeasurement<?>> multiplexed = multiplexMeasurements(independentMeasurements, 1.0e-9);
523 for (ObservedMeasurement<?> measurement : multiplexed) {
524 estimator.addMeasurement(measurement);
525 }
526
527 if (print) {
528 final String headerBLS = "\nBatch Least Square Estimator :\n"
529 + "iteration evaluations ΔP(m) ΔV(m/s) RMS nb Range nb Range-rate nb Angular nb Position nb PV%n";
530 estimator.setObserver(new BatchLeastSquaresObserver(initialGuess, estimator, headerBLS, print));
531 }
532
533
534 final Orbit estimatedBLS = estimator.estimate()[0].getInitialState().getOrbit();
535 final int iterationCount = estimator.getIterationsCount();
536 final int evalutionCount = estimator.getEvaluationsCount();
537 final RealMatrix covariance = estimator.getPhysicalCovariances(1.0e-10);
538
539 Optimum BLSEvaluation = estimator.getOptimum();
540
541
542 estimator = createSequentialEstimator(BLSEvaluation, parser, propagatorBuilder);
543
544
545 independentMeasurements = new ArrayList<>();
546 for (final String fileName : parser.getStringsList(ParameterKey.MEASUREMENTS_FILES_SEQUENTIAL, ',')) {
547
548
549 DataSource nd = new DataSource(fileName, () -> Files.newInputStream(new File(inputModel.getParentFile(), fileName).toPath()));
550 for (final DataFilter filter : Arrays.asList(new GzipFilter(),
551 new UnixCompressFilter(),
552 new HatanakaCompressFilter())) {
553 nd = filter.filter(nd);
554 }
555
556 if (fileName.contains(CPF_MANDATORY_KEY)) {
557
558 independentMeasurements.addAll(readCpf(nd, satellite, initialGuess));
559 }
560
561 }
562
563
564 multiplexed = multiplexMeasurements(independentMeasurements, 1.0e-9);
565 for (ObservedMeasurement<?> measurement : multiplexed) {
566 estimator.addMeasurement(measurement);
567 }
568
569 if (print) {
570 final String headerSBLS = "\nSequentiel Batch Least Square Estimator :\n"
571 + "iteration evaluations ΔP(m) ΔV(m/s) RMS nb Range nb Range-rate nb Angular nb Position nb PV%n";
572
573 estimator.setObserver(new BatchLeastSquaresObserver(initialGuess, estimator, headerSBLS, print));
574 }
575
576 final Orbit estimatedSequentialBLS = estimator.estimate()[0].getInitialState().getOrbit();
577
578
579 for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry : estimator.getLastEstimations().entrySet()) {
580 logEvaluation(entry.getValue(),
581 rangeLog, rangeRateLog, azimuthLog, elevationLog, positionOnlyLog, positionLog, velocityLog);
582 }
583
584 final ParameterDriversList propagatorParameters = estimator.getPropagatorParametersDrivers(true);
585 final ParameterDriversList measurementsParameters = estimator.getMeasurementsParametersDrivers(true);
586
587 return new ResultSequentialBatchLeastSquares(propagatorParameters, measurementsParameters,
588 iterationCount, evalutionCount, estimatedBLS.getPVCoordinates(),
589 positionLog.createStatisticsSummary(),
590 covariance,
591 estimator.getIterationsCount(), estimator.getEvaluationsCount(),
592 estimatedSequentialBLS.getPVCoordinates(), positionLog.createStatisticsSummary(),
593 estimator.getPhysicalCovariances(1.0e-10));
594
595 }
596
597
598
599
600
601
602
603
604
605
606
607
608
609 protected ResultKalman runKalman(final File input, final OrbitType orbitType, final boolean print,
610 final RealMatrix cartesianOrbitalP, final RealMatrix cartesianOrbitalQ,
611 final RealMatrix propagationP, final RealMatrix propagationQ,
612 final RealMatrix measurementP, final RealMatrix measurementQ,
613 final Boolean isUnscented)
614 throws IOException {
615
616
617 KeyValueFileParser<ParameterKey> parser = new KeyValueFileParser<>(ParameterKey.class);
618 parser.parseInput(input.getAbsolutePath(), Files.newInputStream(input.toPath()));
619
620
621 final RangeLog rangeLog = new RangeLog();
622 final RangeRateLog rangeRateLog = new RangeRateLog();
623 final AzimuthLog azimuthLog = new AzimuthLog();
624 final ElevationLog elevationLog = new ElevationLog();
625 final PositionOnlyLog positionOnlyLog = new PositionOnlyLog();
626 final PositionLog positionLog = new PositionLog();
627 final VelocityLog velocityLog = new VelocityLog();
628
629
630 createGravityField(parser);
631
632
633 Orbit initialGuess = createOrbit(parser, getMu());
634
635
636 initialGuess = orbitType.convertType(initialGuess);
637
638
639 final IERSConventions conventions;
640 if (!parser.containsKey(ParameterKey.IERS_CONVENTIONS)) {
641 conventions = IERSConventions.IERS_2010;
642 } else {
643 conventions = IERSConventions.valueOf("IERS_" + parser.getInt(ParameterKey.IERS_CONVENTIONS));
644 }
645
646
647 final OneAxisEllipsoid body = createBody(parser);
648
649
650 final T propagatorBuilder =
651 configurePropagatorBuilder(parser, conventions, body, initialGuess);
652
653
654 final Sinex stationPositionData = readSinexFile(input, parser, ParameterKey.SINEX_POSITION_FILE);
655 final Sinex stationEccData = readSinexFile(input, parser, ParameterKey.SINEX_ECC_FILE);
656
657
658 useRangeMeasurements = parser.getBoolean(ParameterKey.USE_RANGE_MEASUREMENTS);
659 useRangeRateMeasurements = parser.getBoolean(ParameterKey.USE_RANGE_RATE_MEASUREMENTS);
660
661 final Map<String, StationData> stations = createStationsData(parser, initialGuess.getDate(),
662 stationPositionData, stationEccData, conventions, body);
663 final PVData pvData = createPVData(parser);
664 final ObservableSatellite satellite = createObservableSatellite(parser);
665 final Bias<Range> satRangeBias = createSatRangeBias(parser);
666 final PhaseCentersRangeModifier satAntennaRangeModifier = createSatAntennaRangeModifier(parser);
667 final ShapiroRangeModifier shapiroRangeModifier = createShapiroRangeModifier(parser);
668 final Weights weights = createWeights(parser);
669 final OutlierFilter<Range> rangeOutliersManager = createRangeOutliersManager(parser, true);
670 final OutlierFilter<RangeRate> rangeRateOutliersManager = createRangeRateOutliersManager(parser, true);
671 final OutlierFilter<AngularAzEl> azElOutliersManager = createAzElOutliersManager(parser, true);
672 final OutlierFilter<PV> pvOutliersManager = createPVOutliersManager(parser, true);
673
674
675 final List<ObservedMeasurement<?>> independentMeasurements = new ArrayList<>();
676 for (final String fileName : parser.getStringsList(ParameterKey.MEASUREMENTS_FILES, ',')) {
677
678
679 DataSource nd = new DataSource(fileName,
680 () -> Files.newInputStream(new File(input.getParentFile(), fileName).toPath()));
681 for (final DataFilter filter : Arrays.asList(new GzipFilter(),
682 new UnixCompressFilter(),
683 new HatanakaCompressFilter())) {
684 nd = filter.filter(nd);
685 }
686
687 if (Pattern.matches(RinexObservationParser.DEFAULT_RINEX_2_NAMES, nd.getName()) ||
688 Pattern.matches(RinexObservationParser.DEFAULT_RINEX_3_NAMES, nd.getName())) {
689
690 independentMeasurements.addAll(readRinex(nd,
691 parser.getString(ParameterKey.SATELLITE_ID_IN_RINEX_FILES),
692 stations, satellite, satRangeBias, satAntennaRangeModifier, weights,
693 rangeOutliersManager, rangeRateOutliersManager, shapiroRangeModifier));
694 } else if (Pattern.matches(CRDParser.DEFAULT_CRD_SUPPORTED_NAMES, nd.getName())) {
695
696 independentMeasurements.addAll(readCrd(nd, stations, parser, satellite, satRangeBias,
697 weights, rangeOutliersManager, shapiroRangeModifier));
698 } else {
699
700 independentMeasurements.addAll(readMeasurements(nd,
701 stations, pvData, satellite,
702 satRangeBias, satAntennaRangeModifier, weights,
703 rangeOutliersManager,
704 rangeRateOutliersManager,
705 azElOutliersManager,
706 pvOutliersManager));
707 }
708
709 }
710
711 final List<ObservedMeasurement<?>> multiplexed = multiplexMeasurements(independentMeasurements, 1.0e-9);
712
713
714
715 SpacecraftState initialState = propagatorBuilder.buildPropagator()
716 .propagate(multiplexed.get(0).getDate().shiftedBy(-10.0 * 60.0));
717 propagatorBuilder.resetOrbit(initialState.getOrbit());
718
719
720 for (int k = 1; k < multiplexed.size(); ++k) {
721 Assertions.assertTrue(multiplexed.get(k).getDate().isAfter(multiplexed.get(k - 1)));
722 }
723
724
725
726
727
728
729
730
731 final ParameterDriversList estimatedMeasurementsParameters = new ParameterDriversList();
732 for (ObservedMeasurement<?> measurement : multiplexed) {
733 final List<ParameterDriver> drivers = measurement.getParametersDrivers();
734 for (ParameterDriver driver : drivers) {
735 if (driver.isSelected()) {
736
737 estimatedMeasurementsParameters.add(driver);
738 }
739 }
740 }
741
742 estimatedMeasurementsParameters.sort();
743
744
745
746 final double[][] dYdC = new double[6][6];
747 initialGuess.getJacobianWrtCartesian(propagatorBuilder.getPositionAngleType(), dYdC);
748 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
749 RealMatrix orbitalP = Jac.multiply(cartesianOrbitalP.multiply(Jac.transpose()));
750
751
752 RealMatrix orbitalQ = Jac.multiply(cartesianOrbitalQ.multiply(Jac.transpose()));
753
754
755
756 final int nbPropag = (propagationP != null)?propagationP.getRowDimension():0;
757 final int nbMeas = (measurementP != null)?measurementP.getRowDimension():0;
758 final RealMatrix initialP = MatrixUtils.createRealMatrix(6 + nbPropag,
759 6 + nbPropag);
760 final RealMatrix Q = MatrixUtils.createRealMatrix(6 + nbPropag,
761 6 + nbPropag);
762
763 initialP.setSubMatrix(orbitalP.getData(), 0, 0);
764 Q.setSubMatrix(orbitalQ.getData(), 0, 0);
765
766
767 if (propagationP != null) {
768 initialP.setSubMatrix(propagationP.getData(), 6, 6);
769 Q.setSubMatrix(propagationQ.getData(), 6, 6);
770 }
771
772
773 final AbstractKalmanEstimator kalman;
774 if (isUnscented) {
775
776 final UnscentedKalmanEstimatorBuilder kalmanBuilder = new UnscentedKalmanEstimatorBuilder().
777 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q));
778 if (measurementP != null) {
779
780 kalmanBuilder.estimatedMeasurementsParameters(estimatedMeasurementsParameters, new ConstantProcessNoise(measurementP, measurementQ));
781 }
782
783 kalman = kalmanBuilder.unscentedTransformProvider(new MerweUnscentedTransform(6 + nbPropag + nbMeas)).build();
784 } else {
785
786 final KalmanEstimatorBuilder kalmanBuilder = new KalmanEstimatorBuilder().
787 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q));
788 if (measurementP != null) {
789
790 kalmanBuilder.estimatedMeasurementsParameters(estimatedMeasurementsParameters, new ConstantProcessNoise(measurementP, measurementQ));
791 }
792
793 kalman = kalmanBuilder.build();
794 }
795
796
797 Observer observer = new Observer(print, rangeLog, rangeRateLog, azimuthLog, elevationLog, positionOnlyLog, positionLog, velocityLog);
798
799
800 RtsSmoother smoother = new RtsSmoother(kalman);
801
802 List<KalmanObserver> kalmanObservers = new ArrayList<>();
803 kalmanObservers.add(observer);
804 kalmanObservers.add(smoother);
805 kalman.setObserver(new ObserverList(kalmanObservers));
806
807
808 final Orbit estimated;
809 if (isUnscented) {
810 estimated = ((UnscentedKalmanEstimator) kalman).processMeasurements(multiplexed)[0].getInitialState().getOrbit();
811 } else {
812 estimated = ((KalmanEstimator) kalman).processMeasurements(multiplexed)[0].getInitialState().getOrbit();
813 }
814
815
816 final List<PhysicalEstimatedState> smoothedStates = smoother.backwardsSmooth();
817
818
819 final RealMatrix covarianceMatrix = kalman.getPhysicalEstimatedCovarianceMatrix();
820
821
822 final ParameterDriversList propagationParameters = kalman.getPropagationParametersDrivers(true);
823 final ParameterDriversList measurementsParameters = kalman.getEstimatedMeasurementsParameters();
824
825
826 if (print) {
827
828
829 int length = 0;
830 for (final ParameterDriver parameterDriver : propagationParameters.getDrivers()) {
831 length = FastMath.max(length, parameterDriver.getName().length());
832 }
833 for (final ParameterDriver parameterDriver : measurementsParameters.getDrivers()) {
834 length = FastMath.max(length, parameterDriver.getName().length());
835 }
836 if (propagationParameters.getNbParams() > 0) {
837 displayParametersChanges(System.out, "Estimated propagator parameters changes: ",
838 true, length, propagationParameters);
839 }
840 if (measurementsParameters.getNbParams() > 0) {
841 displayParametersChanges(System.out, "Estimated measurements parameters changes: ",
842 true, length, measurementsParameters);
843 }
844
845 System.out.println();
846 rangeLog.displaySummary(System.out);
847 rangeRateLog.displaySummary(System.out);
848 azimuthLog.displaySummary(System.out);
849 elevationLog.displaySummary(System.out);
850 positionOnlyLog.displaySummary(System.out);
851 positionLog.displaySummary(System.out);
852 velocityLog.displaySummary(System.out);
853
854
855 displayFinalCovariances(System.out, kalman);
856 }
857
858
859 return new ResultKalman(propagationParameters, measurementsParameters,
860 kalman.getCurrentMeasurementNumber(), estimated.getPVCoordinates(),
861 rangeLog.createStatisticsSummary(), rangeRateLog.createStatisticsSummary(),
862 azimuthLog.createStatisticsSummary(), elevationLog.createStatisticsSummary(),
863 positionLog.createStatisticsSummary(), velocityLog.createStatisticsSummary(),
864 covarianceMatrix, smoothedStates.get(0));
865
866 }
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881 protected Orbit runReference(final File input, final OrbitType orbitType,
882 final Vector3D refPosition, final Vector3D refVelocity,
883 final ParameterDriversList refPropagationParameters,
884 final AbsoluteDate finalDate) throws IOException {
885
886
887 KeyValueFileParser<ParameterKey> parser = new KeyValueFileParser<>(ParameterKey.class);
888 parser.parseInput(input.getAbsolutePath(), Files.newInputStream(input.toPath()));
889
890
891 createGravityField(parser);
892
893
894 Orbit initialRefOrbit = new CartesianOrbit(new PVCoordinates(refPosition, refVelocity),
895 parser.getInertialFrame(ParameterKey.INERTIAL_FRAME),
896 parser.getDate(ParameterKey.ORBIT_DATE,
897 TimeScalesFactory.getUTC()),
898 getMu());
899
900
901 initialRefOrbit = orbitType.convertType(initialRefOrbit);
902
903
904 final IERSConventions conventions;
905 if (!parser.containsKey(ParameterKey.IERS_CONVENTIONS)) {
906 conventions = IERSConventions.IERS_2010;
907 } else {
908 conventions = IERSConventions.valueOf("IERS_" + parser.getInt(ParameterKey.IERS_CONVENTIONS));
909 }
910
911
912 final OneAxisEllipsoid body = createBody(parser);
913
914
915 final T propagatorBuilder =
916 configurePropagatorBuilder(parser, conventions, body, initialRefOrbit);
917
918
919 if (refPropagationParameters != null) {
920 for (DelegatingDriver refDriver : refPropagationParameters.getDrivers()) {
921 for (DelegatingDriver driver : propagatorBuilder.getPropagationParametersDrivers().getDrivers()) {
922 if (driver.getName().equals(refDriver.getName())) {
923 for (Span<Double> span = driver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
924
925 driver.setValue(refDriver.getValue(initialRefOrbit.getDate()), span.getStart());
926 }
927 }
928 }
929 }
930 }
931
932
933 final Propagator propagator = propagatorBuilder.buildPropagator();
934
935
936 return propagator.propagate(finalDate).getOrbit();
937
938 }
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953 private T configurePropagatorBuilder(final KeyValueFileParser<ParameterKey> parser,
954 final IERSConventions conventions,
955 final OneAxisEllipsoid body,
956 final Orbit orbit)
957 throws NoSuchElementException {
958
959 final double minStep;
960 if (!parser.containsKey(ParameterKey.PROPAGATOR_MIN_STEP)) {
961 minStep = 6000.0;
962 } else {
963 minStep = parser.getDouble(ParameterKey.PROPAGATOR_MIN_STEP);
964 }
965
966 final double maxStep;
967 if (!parser.containsKey(ParameterKey.PROPAGATOR_MAX_STEP)) {
968 maxStep = 86400;
969 } else {
970 maxStep = parser.getDouble(ParameterKey.PROPAGATOR_MAX_STEP);
971 }
972
973 final double dP;
974 if (!parser.containsKey(ParameterKey.PROPAGATOR_POSITION_ERROR)) {
975 dP = 10.0;
976 } else {
977 dP = parser.getDouble(ParameterKey.PROPAGATOR_POSITION_ERROR);
978 }
979
980 final double positionScale;
981 if (!parser.containsKey(ParameterKey.ESTIMATOR_ORBITAL_PARAMETERS_POSITION_SCALE)) {
982 positionScale = dP;
983 } else {
984 positionScale = parser.getDouble(ParameterKey.ESTIMATOR_ORBITAL_PARAMETERS_POSITION_SCALE);
985 }
986
987 final T propagatorBuilder = createPropagatorBuilder(orbit,
988 new DormandPrince853IntegratorBuilder(minStep, maxStep, dP),
989 positionScale);
990
991
992 final double mass;
993 if (!parser.containsKey(ParameterKey.MASS)) {
994 mass = 1000.0;
995 } else {
996 mass = parser.getDouble(ParameterKey.MASS);
997 }
998 setMass(propagatorBuilder, mass);
999
1000 setGravity(propagatorBuilder, body);
1001
1002
1003 if (parser.containsKey(ParameterKey.THIRD_BODY_SUN) &&
1004 parser.getBoolean(ParameterKey.THIRD_BODY_SUN)) {
1005 setThirdBody(propagatorBuilder, CelestialBodyFactory.getSun());
1006 }
1007 if (parser.containsKey(ParameterKey.THIRD_BODY_MOON) &&
1008 parser.getBoolean(ParameterKey.THIRD_BODY_MOON)) {
1009 setThirdBody(propagatorBuilder, CelestialBodyFactory.getMoon());
1010 }
1011
1012
1013 if (parser.containsKey(ParameterKey.OCEAN_TIDES_DEGREE) &&
1014 parser.containsKey(ParameterKey.OCEAN_TIDES_ORDER)) {
1015 final int degree = parser.getInt(ParameterKey.OCEAN_TIDES_DEGREE);
1016 final int order = parser.getInt(ParameterKey.OCEAN_TIDES_ORDER);
1017 if (degree > 0 && order > 0) {
1018 setOceanTides(propagatorBuilder, conventions, body, degree, order);
1019 }
1020 }
1021
1022
1023 final List<CelestialBody> solidTidesBodies = new ArrayList<>();
1024 if (parser.containsKey(ParameterKey.SOLID_TIDES_SUN) &&
1025 parser.getBoolean(ParameterKey.SOLID_TIDES_SUN)) {
1026 solidTidesBodies.add(CelestialBodyFactory.getSun());
1027 }
1028 if (parser.containsKey(ParameterKey.SOLID_TIDES_MOON) &&
1029 parser.getBoolean(ParameterKey.SOLID_TIDES_MOON)) {
1030 solidTidesBodies.add(CelestialBodyFactory.getMoon());
1031 }
1032 if (!solidTidesBodies.isEmpty()) {
1033 setSolidTides(propagatorBuilder, conventions, body,
1034 solidTidesBodies.toArray(new CelestialBody[0]));
1035 }
1036
1037
1038 if (parser.containsKey(ParameterKey.DRAG) && parser.getBoolean(ParameterKey.DRAG)) {
1039 final double cd = parser.getDouble(ParameterKey.DRAG_CD);
1040 final double area = parser.getDouble(ParameterKey.DRAG_AREA);
1041 final boolean cdEstimated = parser.getBoolean(ParameterKey.DRAG_CD_ESTIMATED);
1042
1043 final MarshallSolarActivityFutureEstimation msafe =
1044 new MarshallSolarActivityFutureEstimation(MarshallSolarActivityFutureEstimation.DEFAULT_SUPPORTED_NAMES,
1045 MarshallSolarActivityFutureEstimation.StrengthLevel.AVERAGE);
1046 final Atmosphere atmosphere = new DTM2000(msafe, CelestialBodyFactory.getSun(), body);
1047 final List<ParameterDriver> drivers = setDrag(propagatorBuilder, atmosphere, new IsotropicDrag(area, cd));
1048 if (cdEstimated) {
1049 for (final ParameterDriver driver : drivers) {
1050 if (driver.getName().equals(DragSensitive.DRAG_COEFFICIENT)) {
1051 driver.setSelected(true);
1052 }
1053 }
1054 }
1055 }
1056
1057
1058 if (parser.containsKey(ParameterKey.SOLAR_RADIATION_PRESSURE) && parser.getBoolean(ParameterKey.SOLAR_RADIATION_PRESSURE)) {
1059 final double cr = parser.getDouble(ParameterKey.SOLAR_RADIATION_PRESSURE_CR);
1060 final double area = parser.getDouble(ParameterKey.SOLAR_RADIATION_PRESSURE_AREA);
1061 final boolean cREstimated = parser.getBoolean(ParameterKey.SOLAR_RADIATION_PRESSURE_CR_ESTIMATED);
1062 final List<ParameterDriver> drivers = setSolarRadiationPressure(propagatorBuilder, CelestialBodyFactory.getSun(),
1063 body,
1064 new IsotropicRadiationSingleCoefficient(area, cr));
1065 if (cREstimated) {
1066 for (final ParameterDriver driver : drivers) {
1067 if (driver.getName().equals(RadiationSensitive.REFLECTION_COEFFICIENT)) {
1068 driver.setSelected(true);
1069 }
1070 }
1071 }
1072 }
1073
1074
1075 if (parser.containsKey(ParameterKey.EARTH_ALBEDO_INFRARED) && parser.getBoolean(ParameterKey.EARTH_ALBEDO_INFRARED)) {
1076 final double cr = parser.getDouble(ParameterKey.SOLAR_RADIATION_PRESSURE_CR);
1077 final double area = parser.getDouble(ParameterKey.SOLAR_RADIATION_PRESSURE_AREA);
1078 final boolean cREstimated = parser.getBoolean(ParameterKey.SOLAR_RADIATION_PRESSURE_CR_ESTIMATED);
1079 final double angularResolution = parser.getAngle(ParameterKey.ALBEDO_INFRARED_ANGULAR_RESOLUTION);
1080 final List<ParameterDriver> drivers = setAlbedoInfrared(propagatorBuilder, CelestialBodyFactory.getSun(),
1081 body.getEquatorialRadius(), angularResolution,
1082 new IsotropicRadiationSingleCoefficient(area, cr));
1083 if (cREstimated) {
1084 for (final ParameterDriver driver : drivers) {
1085 if (driver.getName().equals(RadiationSensitive.REFLECTION_COEFFICIENT)) {
1086 driver.setSelected(true);
1087 }
1088 }
1089 }
1090 }
1091
1092
1093 if (parser.containsKey(ParameterKey.GENERAL_RELATIVITY) && parser.getBoolean(ParameterKey.GENERAL_RELATIVITY)) {
1094 setRelativity(propagatorBuilder);
1095 }
1096
1097
1098 if (parser.containsKey(ParameterKey.POLYNOMIAL_ACCELERATION_NAME)) {
1099 final String[] names = parser.getStringArray(ParameterKey.POLYNOMIAL_ACCELERATION_NAME);
1100 final Vector3D[] directions = parser.getVectorArray(ParameterKey.POLYNOMIAL_ACCELERATION_DIRECTION_X,
1101 ParameterKey.POLYNOMIAL_ACCELERATION_DIRECTION_Y,
1102 ParameterKey.POLYNOMIAL_ACCELERATION_DIRECTION_Z);
1103 final List<String>[] coefficients = parser.getStringsListArray(ParameterKey.POLYNOMIAL_ACCELERATION_COEFFICIENTS, ',');
1104 final boolean[] estimated = parser.getBooleanArray(ParameterKey.POLYNOMIAL_ACCELERATION_ESTIMATED);
1105
1106 for (int i = 0; i < names.length; ++i) {
1107 final List<ParameterDriver> drivers = setPolynomialAcceleration(propagatorBuilder, names[i], directions[i], coefficients[i].size() - 1);
1108 for (int k = 0; k < coefficients[i].size(); ++k) {
1109 final String coefficientName = names[i] + "[" + k + "]";
1110 for (final ParameterDriver driver : drivers) {
1111 if (driver.getName().equals(coefficientName)) {
1112 driver.setValue(Double.parseDouble(coefficients[i].get(k)));
1113 driver.setSelected(estimated[i]);
1114 }
1115 }
1116 }
1117 }
1118 }
1119
1120
1121 final AttitudeMode mode;
1122 if (parser.containsKey(ParameterKey.ATTITUDE_MODE)) {
1123 mode = AttitudeMode.valueOf(parser.getString(ParameterKey.ATTITUDE_MODE));
1124 } else {
1125 mode = AttitudeMode.DEFAULT_LAW;
1126 }
1127 setAttitudeProvider(propagatorBuilder, mode.getProvider(orbit.getFrame(), body));
1128
1129 return propagatorBuilder;
1130
1131 }
1132
1133
1134
1135
1136
1137
1138 private OneAxisEllipsoid createBody(final KeyValueFileParser<ParameterKey> parser)
1139 throws NoSuchElementException {
1140
1141 final Frame bodyFrame;
1142 if (!parser.containsKey(ParameterKey.BODY_FRAME)) {
1143 bodyFrame = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
1144 } else {
1145 bodyFrame = parser.getEarthFrame(ParameterKey.BODY_FRAME);
1146 }
1147
1148 final double equatorialRadius;
1149 if (!parser.containsKey(ParameterKey.BODY_EQUATORIAL_RADIUS)) {
1150 equatorialRadius = Constants.WGS84_EARTH_EQUATORIAL_RADIUS;
1151 } else {
1152 equatorialRadius = parser.getDouble(ParameterKey.BODY_EQUATORIAL_RADIUS);
1153 }
1154
1155 final double flattening;
1156 if (!parser.containsKey(ParameterKey.BODY_INVERSE_FLATTENING)) {
1157 flattening = Constants.WGS84_EARTH_FLATTENING;
1158 } else {
1159 flattening = 1.0 / parser.getDouble(ParameterKey.BODY_INVERSE_FLATTENING);
1160 }
1161
1162 return new OneAxisEllipsoid(equatorialRadius, flattening, bodyFrame);
1163 }
1164
1165
1166
1167
1168
1169
1170
1171 private Orbit createOrbit(final KeyValueFileParser<ParameterKey> parser, final double mu)
1172 throws NoSuchElementException {
1173
1174 final Frame frame;
1175 if (!parser.containsKey(ParameterKey.INERTIAL_FRAME)) {
1176 frame = FramesFactory.getEME2000();
1177 } else {
1178 frame = parser.getInertialFrame(ParameterKey.INERTIAL_FRAME);
1179 }
1180
1181
1182 PositionAngleType angleType = PositionAngleType.MEAN;
1183 if (parser.containsKey(ParameterKey.ORBIT_ANGLE_TYPE)) {
1184 angleType = PositionAngleType.valueOf(parser.getString(ParameterKey.ORBIT_ANGLE_TYPE).toUpperCase());
1185 }
1186 if (parser.containsKey(ParameterKey.ORBIT_KEPLERIAN_A)) {
1187 return new KeplerianOrbit(parser.getDouble(ParameterKey.ORBIT_KEPLERIAN_A),
1188 parser.getDouble(ParameterKey.ORBIT_KEPLERIAN_E),
1189 parser.getAngle(ParameterKey.ORBIT_KEPLERIAN_I),
1190 parser.getAngle(ParameterKey.ORBIT_KEPLERIAN_PA),
1191 parser.getAngle(ParameterKey.ORBIT_KEPLERIAN_RAAN),
1192 parser.getAngle(ParameterKey.ORBIT_KEPLERIAN_ANOMALY),
1193 angleType,
1194 frame,
1195 parser.getDate(ParameterKey.ORBIT_DATE,
1196 TimeScalesFactory.getUTC()),
1197 mu);
1198 } else if (parser.containsKey(ParameterKey.ORBIT_EQUINOCTIAL_A)) {
1199 return new EquinoctialOrbit(parser.getDouble(ParameterKey.ORBIT_EQUINOCTIAL_A),
1200 parser.getDouble(ParameterKey.ORBIT_EQUINOCTIAL_EX),
1201 parser.getDouble(ParameterKey.ORBIT_EQUINOCTIAL_EY),
1202 parser.getDouble(ParameterKey.ORBIT_EQUINOCTIAL_HX),
1203 parser.getDouble(ParameterKey.ORBIT_EQUINOCTIAL_HY),
1204 parser.getAngle(ParameterKey.ORBIT_EQUINOCTIAL_LAMBDA),
1205 angleType,
1206 frame,
1207 parser.getDate(ParameterKey.ORBIT_DATE,
1208 TimeScalesFactory.getUTC()),
1209 mu);
1210 } else if (parser.containsKey(ParameterKey.ORBIT_CIRCULAR_A)) {
1211 return new CircularOrbit(parser.getDouble(ParameterKey.ORBIT_CIRCULAR_A),
1212 parser.getDouble(ParameterKey.ORBIT_CIRCULAR_EX),
1213 parser.getDouble(ParameterKey.ORBIT_CIRCULAR_EY),
1214 parser.getAngle(ParameterKey.ORBIT_CIRCULAR_I),
1215 parser.getAngle(ParameterKey.ORBIT_CIRCULAR_RAAN),
1216 parser.getAngle(ParameterKey.ORBIT_CIRCULAR_ALPHA),
1217 angleType,
1218 frame,
1219 parser.getDate(ParameterKey.ORBIT_DATE,
1220 TimeScalesFactory.getUTC()),
1221 mu);
1222 } else if (parser.containsKey(ParameterKey.ORBIT_TLE_LINE_1)) {
1223 final String line1 = parser.getString(ParameterKey.ORBIT_TLE_LINE_1);
1224 final String line2 = parser.getString(ParameterKey.ORBIT_TLE_LINE_2);
1225 final TLE tle = new TLE(line1, line2);
1226
1227 final TLEPropagator propagator = TLEPropagator.selectExtrapolator(tle);
1228
1229 final AbsoluteDate initDate = tle.getDate();
1230 final SpacecraftState initialState = propagator.getInitialState();
1231
1232
1233
1234 return new CartesianOrbit(initialState.getPVCoordinates(FramesFactory.getEME2000()),
1235 frame,
1236 initDate,
1237 mu);
1238
1239
1240 } else {
1241 final double[] pos = {parser.getDouble(ParameterKey.ORBIT_CARTESIAN_PX),
1242 parser.getDouble(ParameterKey.ORBIT_CARTESIAN_PY),
1243 parser.getDouble(ParameterKey.ORBIT_CARTESIAN_PZ)};
1244 final double[] vel = {parser.getDouble(ParameterKey.ORBIT_CARTESIAN_VX),
1245 parser.getDouble(ParameterKey.ORBIT_CARTESIAN_VY),
1246 parser.getDouble(ParameterKey.ORBIT_CARTESIAN_VZ)};
1247
1248 return new CartesianOrbit(new PVCoordinates(new Vector3D(pos), new Vector3D(vel)),
1249 frame,
1250 parser.getDate(ParameterKey.ORBIT_DATE,
1251 TimeScalesFactory.getUTC()),
1252 mu);
1253 }
1254 }
1255
1256
1257
1258
1259
1260 private Bias<Range> createSatRangeBias(final KeyValueFileParser<ParameterKey> parser) {
1261
1262
1263 final double transponderDelayBias;
1264 if (!parser.containsKey(ParameterKey.ONBOARD_RANGE_BIAS)) {
1265 transponderDelayBias = 0;
1266 } else {
1267 transponderDelayBias = parser.getDouble(ParameterKey.ONBOARD_RANGE_BIAS);
1268 }
1269
1270 final double transponderDelayBiasMin;
1271 if (!parser.containsKey(ParameterKey.ONBOARD_RANGE_BIAS_MIN)) {
1272 transponderDelayBiasMin = Double.NEGATIVE_INFINITY;
1273 } else {
1274 transponderDelayBiasMin = parser.getDouble(ParameterKey.ONBOARD_RANGE_BIAS_MIN);
1275 }
1276
1277 final double transponderDelayBiasMax;
1278 if (!parser.containsKey(ParameterKey.ONBOARD_RANGE_BIAS_MAX)) {
1279 transponderDelayBiasMax = Double.NEGATIVE_INFINITY;
1280 } else {
1281 transponderDelayBiasMax = parser.getDouble(ParameterKey.ONBOARD_RANGE_BIAS_MAX);
1282 }
1283
1284
1285 final boolean transponderDelayBiasEstimated;
1286 if (!parser.containsKey(ParameterKey.ONBOARD_RANGE_BIAS_ESTIMATED)) {
1287 transponderDelayBiasEstimated = false;
1288 } else {
1289 transponderDelayBiasEstimated = parser.getBoolean(ParameterKey.ONBOARD_RANGE_BIAS_ESTIMATED);
1290 }
1291
1292 if (FastMath.abs(transponderDelayBias) >= Precision.SAFE_MIN || transponderDelayBiasEstimated) {
1293
1294
1295 final Bias<Range> bias = new Bias<>(new String[] { "transponder delay bias", },
1296 new double[] { transponderDelayBias },
1297 new double[] { 1.0 },
1298 new double[] { transponderDelayBiasMin },
1299 new double[] { transponderDelayBiasMax });
1300 bias.getParametersDrivers().get(0).setSelected(transponderDelayBiasEstimated);
1301 return bias;
1302 } else {
1303
1304 return null;
1305 }
1306 }
1307
1308
1309
1310
1311
1312 private PhaseCentersRangeModifier createSatAntennaRangeModifier(final KeyValueFileParser<ParameterKey> parser) {
1313 final Vector3D offset;
1314 if (!parser.containsKey(ParameterKey.ON_BOARD_ANTENNA_PHASE_CENTER_X)) {
1315 offset = Vector3D.ZERO;
1316 } else {
1317 offset = parser.getVector(ParameterKey.ON_BOARD_ANTENNA_PHASE_CENTER_X,
1318 ParameterKey.ON_BOARD_ANTENNA_PHASE_CENTER_Y,
1319 ParameterKey.ON_BOARD_ANTENNA_PHASE_CENTER_Z);
1320 }
1321 return offset.getNorm() > 0 ?
1322 new PhaseCentersRangeModifier(FrequencyPattern.ZERO_CORRECTION,
1323 new FrequencyPattern(offset, null)) :
1324 null;
1325 }
1326
1327
1328
1329
1330
1331 private ShapiroRangeModifier createShapiroRangeModifier(final KeyValueFileParser<ParameterKey> parser) {
1332 final ShapiroRangeModifier shapiro;
1333 if (!parser.containsKey(ParameterKey.RANGE_SHAPIRO)) {
1334 shapiro = null;
1335 } else {
1336 shapiro = parser.getBoolean(ParameterKey.RANGE_SHAPIRO) ? new ShapiroRangeModifier(getMu()) : null;
1337 }
1338 return shapiro;
1339 }
1340
1341
1342
1343
1344
1345
1346
1347
1348
1349
1350
1351 private Map<String, StationData> createStationsData(final KeyValueFileParser<ParameterKey> parser,
1352 final AbsoluteDate refDate,
1353 final Sinex sinexPosition,
1354 final Sinex sinexEcc,
1355 final IERSConventions conventions,
1356 final OneAxisEllipsoid body)
1357 throws NoSuchElementException {
1358
1359 final Map<String, StationData> stations = new HashMap<>();
1360
1361 final boolean useTimeSpanModel = parser.getBoolean(ParameterKey.USE_TIME_SPAN_TROPOSPHERIC_MODEL);
1362 final String[] stationNames = parser.getStringArray(ParameterKey.GROUND_STATION_NAME);
1363 final double[] stationLatitudes = parser.getAngleArray(ParameterKey.GROUND_STATION_LATITUDE);
1364 final double[] stationLongitudes = parser.getAngleArray(ParameterKey.GROUND_STATION_LONGITUDE);
1365 final double[] stationAltitudes = parser.getDoubleArray(ParameterKey.GROUND_STATION_ALTITUDE);
1366 final boolean[] stationPositionEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_POSITION_ESTIMATED);
1367 final double[] stationClockOffsets = parser.getDoubleArray(ParameterKey.GROUND_STATION_CLOCK_OFFSET);
1368 final double[] stationClockOffsetsMin = parser.getDoubleArray(ParameterKey.GROUND_STATION_CLOCK_OFFSET_MIN);
1369 final double[] stationClockOffsetsMax = parser.getDoubleArray(ParameterKey.GROUND_STATION_CLOCK_OFFSET_MAX);
1370 final boolean[] stationClockOffsetEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_CLOCK_OFFSET_ESTIMATED);
1371 final double[] stationRangeSigma = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_SIGMA);
1372 final double[] stationRangeBias = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_BIAS);
1373 final double[] stationRangeBiasMin = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_BIAS_MIN);
1374 final double[] stationRangeBiasMax = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_BIAS_MAX);
1375 final boolean[] stationRangeBiasEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_RANGE_BIAS_ESTIMATED);
1376 final double[] stationRangeRateSigma = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_SIGMA);
1377 final double[] stationRangeRateBias = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS);
1378 final double[] stationRangeRateBiasMin = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS_MIN);
1379 final double[] stationRangeRateBiasMax = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS_MAX);
1380 final boolean[] stationRangeRateBiasEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS_ESTIMATED);
1381 final double[] stationAzimuthSigma = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_SIGMA);
1382 final double[] stationAzimuthBias = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_BIAS);
1383 final double[] stationAzimuthBiasMin = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_BIAS_MIN);
1384 final double[] stationAzimuthBiasMax = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_BIAS_MAX);
1385 final double[] stationElevationSigma = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_SIGMA);
1386 final double[] stationElevationBias = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_BIAS);
1387 final double[] stationElevationBiasMin = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_BIAS_MIN);
1388 final double[] stationElevationBiasMax = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_BIAS_MAX);
1389 final boolean[] stationAzElBiasesEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_AZ_EL_BIASES_ESTIMATED);
1390 final boolean[] stationElevationRefraction = parser.getBooleanArray(ParameterKey.GROUND_STATION_ELEVATION_REFRACTION_CORRECTION);
1391 final boolean[] stationModelEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_TROPOSPHERIC_MODEL_ESTIMATED);
1392 final double[] stationTroposphericZenithDelay = parser.getDoubleArray(ParameterKey.GROUND_STATION_TROPOSPHERIC_ZENITH_DELAY);
1393 final boolean[] stationZenithDelayEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_TROPOSPHERIC_DELAY_ESTIMATED);
1394 final boolean[] stationGlobalMappingFunction = parser.getBooleanArray(ParameterKey.GROUND_STATION_GLOBAL_MAPPING_FUNCTION);
1395 final boolean[] stationNiellMappingFunction = parser.getBooleanArray(ParameterKey.GROUND_STATION_NIELL_MAPPING_FUNCTION);
1396 final boolean[] stationWeatherEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_WEATHER_ESTIMATED);
1397 final boolean[] stationRangeTropospheric = parser.getBooleanArray(ParameterKey.GROUND_STATION_RANGE_TROPOSPHERIC_CORRECTION);
1398 final boolean[] stationIonosphericCorrection = parser.getBooleanArray(ParameterKey.GROUND_STATION_RANGE_IONOSPHERIC_CORRECTION);
1399 final boolean[] stationIonosphericModelEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_IONOSPHERIC_MODEL_ESTIMATED);
1400 final boolean[] stationVTECEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_IONOSPHERIC_VTEC_ESTIMATED);
1401 final double[] stationIonosphericVTEC = parser.getDoubleArray(ParameterKey.GROUND_STATION_IONOSPHERIC_VTEC_VALUE);
1402 final double[] stationIonosphericHIon = parser.getDoubleArray(ParameterKey.GROUND_STATION_IONOSPHERIC_HION_VALUE);
1403
1404 final TidalDisplacement tidalDisplacement;
1405 if (parser.containsKey(ParameterKey.SOLID_TIDES_DISPLACEMENT_CORRECTION) &&
1406 parser.getBoolean(ParameterKey.SOLID_TIDES_DISPLACEMENT_CORRECTION)) {
1407 final boolean removePermanentDeformation =
1408 parser.containsKey(ParameterKey.SOLID_TIDES_DISPLACEMENT_REMOVE_PERMANENT_DEFORMATION) &&
1409 parser.getBoolean(ParameterKey.SOLID_TIDES_DISPLACEMENT_REMOVE_PERMANENT_DEFORMATION);
1410 tidalDisplacement = new TidalDisplacement(Constants.EIGEN5C_EARTH_EQUATORIAL_RADIUS,
1411 Constants.JPL_SSD_SUN_EARTH_PLUS_MOON_MASS_RATIO,
1412 Constants.JPL_SSD_EARTH_MOON_MASS_RATIO,
1413 CelestialBodyFactory.getSun(),
1414 CelestialBodyFactory.getMoon(),
1415 conventions,
1416 removePermanentDeformation);
1417 } else {
1418 tidalDisplacement = null;
1419 }
1420
1421 final OceanLoadingCoefficientsBLQFactory blqFactory;
1422 if (parser.containsKey(ParameterKey.OCEAN_LOADING_CORRECTION) &&
1423 parser.getBoolean(ParameterKey.OCEAN_LOADING_CORRECTION)) {
1424 blqFactory = new OceanLoadingCoefficientsBLQFactory("^.*\\.blq$");
1425 } else {
1426 blqFactory = null;
1427 }
1428
1429 final EOPHistory eopHistory = FramesFactory.findEOP(body.getBodyFrame());
1430 Geoid geoid = new Geoid(GravityFieldFactory.getNormalizedProvider(9, 9),
1431 ReferenceEllipsoid.getWgs84(body.getBodyFrame()));
1432 final GlobalPressureTemperature gpt = new GlobalPressureTemperature(geoid, TimeScalesFactory.getUTC());
1433 for (int i = 0; i < stationNames.length; ++i) {
1434
1435
1436 final StationDisplacement[] displacements;
1437 final OceanLoading oceanLoading = (blqFactory == null) ?
1438 null :
1439 new OceanLoading(body, blqFactory.getCoefficients(stationNames[i]));
1440 if (tidalDisplacement == null) {
1441 if (oceanLoading == null) {
1442 displacements = new StationDisplacement[0];
1443 } else {
1444 displacements = new StationDisplacement[] {
1445 oceanLoading
1446 };
1447 }
1448 } else {
1449 if (oceanLoading == null) {
1450 displacements = new StationDisplacement[] {
1451 tidalDisplacement
1452 };
1453 } else {
1454 displacements = new StationDisplacement[] {
1455 tidalDisplacement, oceanLoading
1456 };
1457 }
1458 }
1459
1460
1461 final GeodeticPoint position;
1462 if (sinexPosition != null) {
1463
1464 final Station stationData = sinexPosition.getStations().get(stationNames[i].substring(0, 4));
1465 position = body.transform(stationData.getPosition(), body.getBodyFrame(), stationData.getEpoch());
1466 } else {
1467
1468 position = new GeodeticPoint(stationLatitudes[i], stationLongitudes[i], stationAltitudes[i]);
1469 }
1470 final TopocentricFrame topo = new TopocentricFrame(body, position, stationNames[i]);
1471 final PressureTemperatureHumidityProvider pth0Provider =
1472 TroposphericModelUtils.STANDARD_ATMOSPHERE_PROVIDER;
1473 final GroundStation station = new GroundStation(topo, eopHistory, displacements);
1474 station.getClockOffsetDriver().setReferenceValue(stationClockOffsets[i]);
1475 station.getClockOffsetDriver().setValue(stationClockOffsets[i]);
1476 station.getClockOffsetDriver().setMinValue(stationClockOffsetsMin[i]);
1477 station.getClockOffsetDriver().setMaxValue(stationClockOffsetsMax[i]);
1478 station.getClockOffsetDriver().setSelected(stationClockOffsetEstimated[i]);
1479 station.getEastOffsetDriver().setSelected(stationPositionEstimated[i]);
1480 station.getNorthOffsetDriver().setSelected(stationPositionEstimated[i]);
1481 station.getZenithOffsetDriver().setSelected(stationPositionEstimated[i]);
1482
1483
1484 if (sinexEcc != null) {
1485 final Station stationEcc = sinexEcc.getStations().get(stationNames[i]);
1486 final Vector3D eccentricities;
1487 if (stationEcc.getEccRefSystem() == Station.ReferenceSystem.UNE) {
1488 eccentricities = stationEcc.getEccentricities(refDate);
1489 } else {
1490 final Vector3D xyz = stationEcc.getEccentricities(refDate);
1491 eccentricities = new Vector3D(Vector3D.dotProduct(xyz, position.getZenith()),
1492 Vector3D.dotProduct(xyz, position.getNorth()),
1493 Vector3D.dotProduct(xyz, position.getEast()));
1494 }
1495 station.getZenithOffsetDriver().setValue(eccentricities.getX());
1496 station.getZenithOffsetDriver().setReferenceValue(eccentricities.getX());
1497 station.getNorthOffsetDriver().setValue(eccentricities.getY());
1498 station.getNorthOffsetDriver().setReferenceValue(eccentricities.getY());
1499 station.getEastOffsetDriver().setValue(eccentricities.getZ());
1500 station.getEastOffsetDriver().setReferenceValue(eccentricities.getZ());
1501 }
1502
1503
1504 final double rangeSigma = stationRangeSigma[i];
1505 final Bias<Range> rangeBias;
1506 if (FastMath.abs(stationRangeBias[i]) >= Precision.SAFE_MIN || stationRangeBiasEstimated[i]) {
1507 rangeBias = new Bias<>(new String[] { stationNames[i] + RANGE_BIAS_SUFFIX, },
1508 new double[] { stationRangeBias[i] },
1509 new double[] { rangeSigma },
1510 new double[] { stationRangeBiasMin[i] },
1511 new double[] { stationRangeBiasMax[i] });
1512 rangeBias.getParametersDrivers().get(0).setSelected(stationRangeBiasEstimated[i]);
1513 } else {
1514
1515 rangeBias = null;
1516 }
1517
1518
1519 final double rangeRateSigma = stationRangeRateSigma[i];
1520 final Bias<RangeRate> rangeRateBias;
1521 if (FastMath.abs(stationRangeRateBias[i]) >= Precision.SAFE_MIN || stationRangeRateBiasEstimated[i]) {
1522 rangeRateBias = new Bias<>(new String[] { stationNames[i] + RANGE_RATE_BIAS_SUFFIX },
1523 new double[] { stationRangeRateBias[i] },
1524 new double[] { rangeRateSigma },
1525 new double[] { stationRangeRateBiasMin[i] },
1526 new double[] {
1527 stationRangeRateBiasMax[i]
1528 });
1529 rangeRateBias.getParametersDrivers().get(0).setSelected(stationRangeRateBiasEstimated[i]);
1530 } else {
1531
1532 rangeRateBias = null;
1533 }
1534
1535
1536 final double[] azELSigma = new double[] {
1537 stationAzimuthSigma[i], stationElevationSigma[i]
1538 };
1539 final Bias<AngularAzEl> azELBias;
1540 if (FastMath.abs(stationAzimuthBias[i]) >= Precision.SAFE_MIN ||
1541 FastMath.abs(stationElevationBias[i]) >= Precision.SAFE_MIN ||
1542 stationAzElBiasesEstimated[i]) {
1543 azELBias = new Bias<>(new String[] { stationNames[i] + AZIMUTH_BIAS_SUFFIX,
1544 stationNames[i] + ELEVATION_BIAS_SUFFIX },
1545 new double[] { stationAzimuthBias[i], stationElevationBias[i] },
1546 azELSigma,
1547 new double[] { stationAzimuthBiasMin[i], stationElevationBiasMin[i] },
1548 new double[] { stationAzimuthBiasMax[i], stationElevationBiasMax[i] });
1549 azELBias.getParametersDrivers().get(0).setSelected(stationAzElBiasesEstimated[i]);
1550 azELBias.getParametersDrivers().get(1).setSelected(stationAzElBiasesEstimated[i]);
1551 } else {
1552
1553 azELBias = null;
1554 }
1555
1556
1557 final AngularRadioRefractionModifier refractionCorrection;
1558 if (stationElevationRefraction[i]) {
1559 final double altitude = station.getBaseFrame().getPoint().getAltitude();
1560 final AtmosphericRefractionModel refractionModel = new ITURP834AtmosphericRefraction(altitude);
1561 refractionCorrection = new AngularRadioRefractionModifier(refractionModel);
1562 } else {
1563 refractionCorrection = null;
1564 }
1565
1566
1567
1568 final RangeTroposphericDelayModifier rangeTroposphericCorrection;
1569 if (stationRangeTropospheric[i]) {
1570
1571 TroposphereMappingFunction mappingModel = null;
1572 if (stationGlobalMappingFunction[i]) {
1573 mappingModel = new GlobalMappingFunctionModel();
1574 } else if (stationNiellMappingFunction[i]) {
1575 mappingModel = new NiellMappingFunctionModel();
1576 }
1577
1578 final TroposphericModel model;
1579 if (stationModelEstimated[i] && mappingModel != null) {
1580
1581
1582 if (useTimeSpanModel) {
1583
1584 final EstimatedModel initialModel = new EstimatedModel(new ModifiedSaastamoinenModel(pth0Provider),
1585 mappingModel,
1586 stationTroposphericZenithDelay[i]);
1587
1588
1589 final TimeSpanEstimatedModel timeSpanModel = new TimeSpanEstimatedModel(initialModel);
1590
1591
1592 final AbsoluteDate epoch = parser.getDate(ParameterKey.TROPOSPHERIC_CORRECTION_DATE, TimeScalesFactory.getUTC());
1593
1594
1595 final String subName = stationNames[i].substring(0, 5);
1596
1597
1598 final EstimatedModel modelBefore = new EstimatedModel(new ModifiedSaastamoinenModel(pth0Provider),
1599 mappingModel,
1600 stationTroposphericZenithDelay[i]);
1601 final ParameterDriver totalDelayBefore = modelBefore.getParametersDrivers().get(0);
1602 totalDelayBefore.setSelected(stationZenithDelayEstimated[i]);
1603 totalDelayBefore.setName(subName + TimeSpanEstimatedModel.DATE_BEFORE + epoch.toString(TimeScalesFactory.getUTC()) + " " + EstimatedModel.TOTAL_ZENITH_DELAY);
1604
1605
1606 final EstimatedModel modelAfter = new EstimatedModel(new ModifiedSaastamoinenModel(pth0Provider),
1607 mappingModel,
1608 stationTroposphericZenithDelay[i]);
1609 final ParameterDriver totalDelayAfter = modelAfter.getParametersDrivers().get(0);
1610 totalDelayAfter.setSelected(stationZenithDelayEstimated[i]);
1611 totalDelayAfter.setName(subName + TimeSpanEstimatedModel.DATE_AFTER +
1612 epoch.toString(TimeScalesFactory.getUTC()) + " " +
1613 EstimatedModel.TOTAL_ZENITH_DELAY);
1614
1615
1616
1617
1618
1619
1620 if (subName.equals("SEAT/")) {
1621
1622
1623 } else {
1624 timeSpanModel.addTroposphericModelValidBefore(modelBefore, epoch);
1625 }
1626 if (subName.equals("BADG/") || subName.equals("IRKM/")) {
1627
1628
1629 } else {
1630 timeSpanModel.addTroposphericModelValidAfter(modelAfter, epoch);
1631 }
1632
1633 model = timeSpanModel;
1634
1635 } else {
1636
1637 model = new EstimatedModel(new ModifiedSaastamoinenModel(pth0Provider),
1638 mappingModel, stationTroposphericZenithDelay[i]);
1639 final ParameterDriver driver = model.getParametersDrivers().get(0);
1640 driver.setName(stationNames[i].substring(0, 4) + "/ " + EstimatedModel.TOTAL_ZENITH_DELAY);
1641 driver.setSelected(stationZenithDelayEstimated[i]);
1642
1643 }
1644
1645 } else {
1646
1647 model = new ModifiedSaastamoinenModel(TroposphericModelUtils.STANDARD_ATMOSPHERE_PROVIDER);
1648 }
1649
1650 rangeTroposphericCorrection = new RangeTroposphericDelayModifier(model);
1651 } else {
1652 rangeTroposphericCorrection = null;
1653 }
1654
1655
1656 final IonosphericModel ionosphericModel;
1657 if (stationIonosphericCorrection[i]) {
1658 if (stationIonosphericModelEstimated[i]) {
1659
1660 final IonosphericMappingFunction mapping = new SingleLayerModelMappingFunction(stationIonosphericHIon[i]);
1661 ionosphericModel = new EstimatedIonosphericModel(mapping, stationIonosphericVTEC[i]);
1662 final ParameterDriver ionosphericDriver = ionosphericModel.getParametersDrivers().get(0);
1663 ionosphericDriver.setSelected(stationVTECEstimated[i]);
1664 ionosphericDriver.setName(stationNames[i].substring(0, 5) + EstimatedIonosphericModel.VERTICAL_TOTAL_ELECTRON_CONTENT);
1665 } else {
1666 final TimeScale utc = TimeScalesFactory.getUTC();
1667
1668 final KlobucharIonoCoefficientsLoader loader = new KlobucharIonoCoefficientsLoader();
1669 loader.loadKlobucharIonosphericCoefficients(parser.getDate(ParameterKey.ORBIT_DATE, utc).getComponents(utc).getDate());
1670 ionosphericModel = new KlobucharIonoModel(loader.getAlpha(), loader.getBeta());
1671 }
1672 } else {
1673 ionosphericModel = null;
1674 }
1675
1676 stations.put(stationNames[i],
1677 new StationData(station,
1678 rangeSigma, rangeBias,
1679 rangeRateSigma, rangeRateBias,
1680 azELSigma, azELBias,
1681 refractionCorrection, rangeTroposphericCorrection,
1682 ionosphericModel));
1683 }
1684 return stations;
1685
1686 }
1687
1688
1689
1690
1691
1692
1693 private Weights createWeights(final KeyValueFileParser<ParameterKey> parser)
1694 throws NoSuchElementException {
1695 return new Weights(parser.getDouble(ParameterKey.RANGE_MEASUREMENTS_BASE_WEIGHT),
1696 parser.getDouble(ParameterKey.RANGE_RATE_MEASUREMENTS_BASE_WEIGHT),
1697 new double[] {
1698 parser.getDouble(ParameterKey.AZIMUTH_MEASUREMENTS_BASE_WEIGHT),
1699 parser.getDouble(ParameterKey.ELEVATION_MEASUREMENTS_BASE_WEIGHT)
1700 },
1701 parser.getDouble(ParameterKey.PV_MEASUREMENTS_BASE_WEIGHT));
1702 }
1703
1704
1705
1706
1707
1708
1709 private OutlierFilter<Range> createRangeOutliersManager(final KeyValueFileParser<ParameterKey> parser, boolean isDynamic) {
1710 needsBothOrNeither(parser,
1711 ParameterKey.RANGE_OUTLIER_REJECTION_MULTIPLIER,
1712 ParameterKey.RANGE_OUTLIER_REJECTION_STARTING_ITERATION);
1713 return isDynamic ?
1714 new DynamicOutlierFilter<>(parser.getInt(ParameterKey.RANGE_OUTLIER_REJECTION_STARTING_ITERATION),
1715 parser.getInt(ParameterKey.RANGE_OUTLIER_REJECTION_MULTIPLIER)) :
1716 new OutlierFilter<>(parser.getInt(ParameterKey.RANGE_OUTLIER_REJECTION_STARTING_ITERATION),
1717 parser.getInt(ParameterKey.RANGE_OUTLIER_REJECTION_MULTIPLIER));
1718 }
1719
1720
1721
1722
1723
1724
1725 private OutlierFilter<RangeRate> createRangeRateOutliersManager(final KeyValueFileParser<ParameterKey> parser, boolean isDynamic) {
1726 needsBothOrNeither(parser,
1727 ParameterKey.RANGE_RATE_OUTLIER_REJECTION_MULTIPLIER,
1728 ParameterKey.RANGE_RATE_OUTLIER_REJECTION_STARTING_ITERATION);
1729 return isDynamic ?
1730 new DynamicOutlierFilter<>(parser.getInt(ParameterKey.RANGE_RATE_OUTLIER_REJECTION_STARTING_ITERATION),
1731 parser.getInt(ParameterKey.RANGE_RATE_OUTLIER_REJECTION_MULTIPLIER)) :
1732 new OutlierFilter<>(parser.getInt(ParameterKey.RANGE_RATE_OUTLIER_REJECTION_STARTING_ITERATION),
1733 parser.getInt(ParameterKey.RANGE_RATE_OUTLIER_REJECTION_MULTIPLIER));
1734 }
1735
1736
1737
1738
1739
1740
1741 private OutlierFilter<AngularAzEl> createAzElOutliersManager(final KeyValueFileParser<ParameterKey> parser, boolean isDynamic) {
1742 needsBothOrNeither(parser,
1743 ParameterKey.AZ_EL_OUTLIER_REJECTION_MULTIPLIER,
1744 ParameterKey.AZ_EL_OUTLIER_REJECTION_STARTING_ITERATION);
1745 return isDynamic ?
1746 new DynamicOutlierFilter<>(parser.getInt(ParameterKey.AZ_EL_OUTLIER_REJECTION_STARTING_ITERATION),
1747 parser.getInt(ParameterKey.AZ_EL_OUTLIER_REJECTION_MULTIPLIER)) :
1748 new OutlierFilter<>(parser.getInt(ParameterKey.AZ_EL_OUTLIER_REJECTION_STARTING_ITERATION),
1749 parser.getInt(ParameterKey.AZ_EL_OUTLIER_REJECTION_MULTIPLIER));
1750 }
1751
1752
1753
1754
1755
1756
1757 private OutlierFilter<PV> createPVOutliersManager(final KeyValueFileParser<ParameterKey> parser, boolean isDynamic) {
1758 needsBothOrNeither(parser,
1759 ParameterKey.PV_OUTLIER_REJECTION_MULTIPLIER,
1760 ParameterKey.PV_OUTLIER_REJECTION_STARTING_ITERATION);
1761 return isDynamic ?
1762 new DynamicOutlierFilter<>(parser.getInt(ParameterKey.PV_OUTLIER_REJECTION_STARTING_ITERATION),
1763 parser.getInt(ParameterKey.PV_OUTLIER_REJECTION_MULTIPLIER)) :
1764 new OutlierFilter<>(parser.getInt(ParameterKey.PV_OUTLIER_REJECTION_STARTING_ITERATION),
1765 parser.getInt(ParameterKey.PV_OUTLIER_REJECTION_MULTIPLIER));
1766 }
1767
1768
1769
1770
1771
1772
1773 private PVData createPVData(final KeyValueFileParser<ParameterKey> parser)
1774 throws NoSuchElementException {
1775 return new PVData(parser.getDouble(ParameterKey.PV_MEASUREMENTS_POSITION_SIGMA),
1776 parser.getDouble(ParameterKey.PV_MEASUREMENTS_VELOCITY_SIGMA));
1777 }
1778
1779
1780
1781
1782
1783
1784 private ObservableSatellite createObservableSatellite(final KeyValueFileParser<ParameterKey> parser)
1785 throws NoSuchElementException {
1786 final ObservableSatellite obsSat = new ObservableSatellite(0);
1787 final ParameterDriver clockOffsetDriver = obsSat.getClockOffsetDriver();
1788 if (parser.containsKey(ParameterKey.ON_BOARD_CLOCK_OFFSET)) {
1789
1790 clockOffsetDriver.setReferenceValue(parser.getDouble(ParameterKey.ON_BOARD_CLOCK_OFFSET));
1791 clockOffsetDriver.setValue(parser.getDouble(ParameterKey.ON_BOARD_CLOCK_OFFSET));
1792 }
1793 if (parser.containsKey(ParameterKey.ON_BOARD_CLOCK_OFFSET_MIN)) {
1794 clockOffsetDriver.setMinValue(parser.getDouble(ParameterKey.ON_BOARD_CLOCK_OFFSET_MIN));
1795 }
1796 if (parser.containsKey(ParameterKey.ON_BOARD_CLOCK_OFFSET_MAX)) {
1797 clockOffsetDriver.setMaxValue(parser.getDouble(ParameterKey.ON_BOARD_CLOCK_OFFSET_MAX));
1798 }
1799 if (parser.containsKey(ParameterKey.ON_BOARD_CLOCK_OFFSET_ESTIMATED)) {
1800 clockOffsetDriver.setSelected(parser.getBoolean(ParameterKey.ON_BOARD_CLOCK_OFFSET_ESTIMATED));
1801 }
1802 return obsSat;
1803 }
1804
1805
1806
1807
1808
1809
1810
1811 private BatchLSEstimator createEstimator(final KeyValueFileParser<ParameterKey> parser,
1812 final PropagatorBuilder propagatorBuilder)
1813 throws NoSuchElementException {
1814
1815 final boolean optimizerIsLevenbergMarquardt;
1816 if (!parser.containsKey(ParameterKey.ESTIMATOR_OPTIMIZATION_ENGINE)) {
1817 optimizerIsLevenbergMarquardt = true;
1818 } else {
1819 final String engine = parser.getString(ParameterKey.ESTIMATOR_OPTIMIZATION_ENGINE);
1820 optimizerIsLevenbergMarquardt = engine.toLowerCase().contains("levenberg");
1821 }
1822 final LeastSquaresOptimizer optimizer;
1823
1824 if (optimizerIsLevenbergMarquardt) {
1825
1826 final double initialStepBoundFactor;
1827 if (!parser.containsKey(ParameterKey.ESTIMATOR_LEVENBERG_MARQUARDT_INITIAL_STEP_BOUND_FACTOR)) {
1828 initialStepBoundFactor = 100.0;
1829 } else {
1830 initialStepBoundFactor = parser.getDouble(ParameterKey.ESTIMATOR_LEVENBERG_MARQUARDT_INITIAL_STEP_BOUND_FACTOR);
1831 }
1832
1833 optimizer = new LevenbergMarquardtOptimizer().withInitialStepBoundFactor(initialStepBoundFactor);
1834 } else {
1835
1836 optimizer = new GaussNewtonOptimizer(new QRDecomposer(1e-11), false);
1837 }
1838
1839 final double convergenceThreshold;
1840 if (!parser.containsKey(ParameterKey.ESTIMATOR_NORMALIZED_PARAMETERS_CONVERGENCE_THRESHOLD)) {
1841 convergenceThreshold = 1.0e-3;
1842 } else {
1843 convergenceThreshold = parser.getDouble(ParameterKey.ESTIMATOR_NORMALIZED_PARAMETERS_CONVERGENCE_THRESHOLD);
1844 }
1845 final int maxIterations;
1846 if (!parser.containsKey(ParameterKey.ESTIMATOR_MAX_ITERATIONS)) {
1847 maxIterations = 10;
1848 } else {
1849 maxIterations = parser.getInt(ParameterKey.ESTIMATOR_MAX_ITERATIONS);
1850 }
1851 final int maxEvaluations;
1852 if (!parser.containsKey(ParameterKey.ESTIMATOR_MAX_EVALUATIONS)) {
1853 maxEvaluations = 20;
1854 } else {
1855 maxEvaluations = parser.getInt(ParameterKey.ESTIMATOR_MAX_EVALUATIONS);
1856 }
1857
1858 final BatchLSEstimator estimator = new BatchLSEstimator(optimizer, propagatorBuilder);
1859 estimator.setParametersConvergenceThreshold(convergenceThreshold);
1860 estimator.setMaxIterations(maxIterations);
1861 estimator.setMaxEvaluations(maxEvaluations);
1862
1863 return estimator;
1864
1865 }
1866
1867
1868
1869
1870
1871
1872
1873 private BatchLSEstimator createSequentialEstimator(final Optimum optimum, final KeyValueFileParser<ParameterKey> parser,
1874 final PropagatorBuilder propagatorBuilder)
1875 throws NoSuchElementException {
1876
1877
1878 final double convergenceThreshold;
1879 if (!parser.containsKey(ParameterKey.ESTIMATOR_NORMALIZED_PARAMETERS_CONVERGENCE_THRESHOLD)) {
1880 convergenceThreshold = 1.0e-3;
1881 } else {
1882 convergenceThreshold = parser.getDouble(ParameterKey.ESTIMATOR_NORMALIZED_PARAMETERS_CONVERGENCE_THRESHOLD);
1883 }
1884 final int maxIterations;
1885 if (!parser.containsKey(ParameterKey.ESTIMATOR_MAX_ITERATIONS)) {
1886 maxIterations = 10;
1887 } else {
1888 maxIterations = parser.getInt(ParameterKey.ESTIMATOR_MAX_ITERATIONS);
1889 }
1890 final int maxEvaluations;
1891 if (!parser.containsKey(ParameterKey.ESTIMATOR_MAX_EVALUATIONS)) {
1892 maxEvaluations = 20;
1893 } else {
1894 maxEvaluations = parser.getInt(ParameterKey.ESTIMATOR_MAX_EVALUATIONS);
1895 }
1896
1897 final SequentialGaussNewtonOptimizer optimizer = new SequentialGaussNewtonOptimizer().withEvaluation(optimum);
1898 final SequentialBatchLSEstimator estimator = new SequentialBatchLSEstimator(optimizer, propagatorBuilder);
1899 estimator.setParametersConvergenceThreshold(convergenceThreshold);
1900 estimator.setMaxIterations(maxIterations);
1901 estimator.setMaxEvaluations(maxEvaluations);
1902
1903 return estimator;
1904
1905 }
1906
1907
1908
1909
1910
1911
1912
1913
1914 private Sinex readSinexFile(final File input,
1915 final KeyValueFileParser<ParameterKey> parser,
1916 final ParameterKey key)
1917 throws IOException {
1918
1919
1920 if (parser.containsKey(key)) {
1921
1922
1923 final String fileName = parser.getString(key);
1924
1925
1926 DataSource nd = new DataSource(fileName,
1927 () -> Files.newInputStream(new File(input.getParentFile(), fileName).toPath()));
1928 for (final DataFilter filter : Arrays.asList(new GzipFilter(),
1929 new UnixCompressFilter(),
1930 new HatanakaCompressFilter())) {
1931 nd = filter.filter(nd);
1932 }
1933
1934
1935 return new SinexParser(TimeScalesFactory.getTimeScales()).parse(nd);
1936
1937 } else {
1938
1939
1940 return null;
1941
1942 }
1943
1944 }
1945
1946
1947
1948
1949
1950
1951
1952
1953
1954
1955
1956
1957
1958
1959
1960
1961 private List<ObservedMeasurement<?>> readMeasurements(final DataSource source,
1962 final Map<String, StationData> stations,
1963 final PVData pvData,
1964 final ObservableSatellite satellite,
1965 final Bias<Range> satRangeBias,
1966 final PhaseCentersRangeModifier satAntennaRangeModifier,
1967 final Weights weights,
1968 final OutlierFilter<Range> rangeOutliersManager,
1969 final OutlierFilter<RangeRate> rangeRateOutliersManager,
1970 final OutlierFilter<AngularAzEl> azElOutliersManager,
1971 final OutlierFilter<PV> pvOutliersManager)
1972 throws IOException {
1973
1974 final List<ObservedMeasurement<?>> measurements = new ArrayList<>();
1975 try (Reader reader = source.getOpener().openReaderOnce();
1976 BufferedReader br = new BufferedReader(reader)) {
1977 int lineNumber = 0;
1978 for (String line = br.readLine(); line != null; line = br.readLine()) {
1979 ++lineNumber;
1980 line = line.trim();
1981 if (!line.isEmpty() && !line.startsWith("#")) {
1982 final String[] fields = line.split("\\s+");
1983 if (fields.length < 2) {
1984 throw new OrekitException(OrekitMessages.UNABLE_TO_PARSE_LINE_IN_FILE,
1985 lineNumber, source.getName(), line);
1986 }
1987 switch (fields[1]) {
1988 case "RANGE" :
1989 if (useRangeMeasurements) {
1990 final Range range = new RangeParser().parseFields(fields, stations, pvData, satellite,
1991 satRangeBias, weights,
1992 line, lineNumber, source.getName());
1993 if (satAntennaRangeModifier != null) {
1994 range.addModifier(satAntennaRangeModifier);
1995 }
1996 if (rangeOutliersManager != null) {
1997 range.addModifier(rangeOutliersManager);
1998 }
1999 addIfNonZeroWeight(range, measurements);
2000 }
2001 break;
2002 case "RANGE_RATE" :
2003 if (useRangeRateMeasurements) {
2004 final RangeRate rangeRate = new RangeRateParser().parseFields(fields, stations, pvData, satellite,
2005 satRangeBias, weights,
2006 line, lineNumber, source.getName());
2007 if (rangeRateOutliersManager != null) {
2008 rangeRate.addModifier(rangeRateOutliersManager);
2009 }
2010 addIfNonZeroWeight(rangeRate, measurements);
2011 }
2012 break;
2013 case "AZ_EL" :
2014 final AngularAzEl angular = new AzElParser().parseFields(fields, stations, pvData, satellite,
2015 satRangeBias, weights,
2016 line, lineNumber, source.getName());
2017 if (azElOutliersManager != null) {
2018 angular.addModifier(azElOutliersManager);
2019 }
2020 addIfNonZeroWeight(angular, measurements);
2021 break;
2022 case "PV" :
2023 final PV pv = new PVParser().parseFields(fields, stations, pvData, satellite,
2024 satRangeBias, weights,
2025 line, lineNumber, source.getName());
2026 if (pvOutliersManager != null) {
2027 pv.addModifier(pvOutliersManager);
2028 }
2029 addIfNonZeroWeight(pv, measurements);
2030 break;
2031 default :
2032 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
2033 "unknown measurement type " + fields[1] +
2034 " at line " + lineNumber +
2035 " in file " + source.getName() +
2036 "\n" + line);
2037 }
2038 }
2039 }
2040 }
2041
2042 if (measurements.isEmpty()) {
2043 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
2044 "not measurements read from file " + source.getName());
2045 }
2046
2047 return measurements;
2048
2049 }
2050
2051
2052
2053
2054
2055
2056
2057
2058
2059
2060
2061
2062
2063
2064 private List<ObservedMeasurement<?>> readRinex(final DataSource source, final String satId,
2065 final Map<String, StationData> stations,
2066 final ObservableSatellite satellite,
2067 final Bias<Range> satRangeBias,
2068 final PhaseCentersRangeModifier satAntennaRangeModifier,
2069 final Weights weights,
2070 final OutlierFilter<Range> rangeOutliersManager,
2071 final OutlierFilter<RangeRate> rangeRateOutliersManager,
2072 final ShapiroRangeModifier shapiroRangeModifier) {
2073 final String notConfigured = " not configured";
2074 final List<ObservedMeasurement<?>> measurements = new ArrayList<>();
2075 final SatelliteSystem system = SatelliteSystem.parseSatelliteSystem(satId);
2076 final int prnNumber;
2077 switch (system) {
2078 case GPS:
2079 case GLONASS:
2080 case GALILEO:
2081 prnNumber = Integer.parseInt(satId.substring(1));
2082 break;
2083 case SBAS:
2084 prnNumber = Integer.parseInt(satId.substring(1)) + 100;
2085 break;
2086 default:
2087 prnNumber = -1;
2088 }
2089 final RinexObservation rinexObs = new RinexObservationParser().parse(source);
2090 for (final ObservationDataSet observationDataSet : rinexObs.getObservationDataSets()) {
2091 if (observationDataSet.getSatellite().getSystem() == system &&
2092 observationDataSet.getSatellite().getPRN() == prnNumber) {
2093 for (final ObservationData od : observationDataSet.getObservationData()) {
2094 final double snr = od.getSignalStrength();
2095 if (!Double.isNaN(od.getValue()) && (snr == 0 || snr >= 4)) {
2096 if (od.getObservationType().getMeasurementType() == MeasurementType.PSEUDO_RANGE && useRangeMeasurements) {
2097
2098 final String stationName = rinexObs.getHeader().getMarkerName() + "/" + od.getObservationType();
2099 final StationData stationData = stations.get(stationName);
2100 if (stationData == null) {
2101 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
2102 stationName + notConfigured);
2103 }
2104 final Range range = new Range(stationData.getStation(), false, observationDataSet.getDate(),
2105 od.getValue(), stationData.getRangeSigma(),
2106 weights.getRangeBaseWeight(), satellite);
2107 if (stationData.getIonosphericModel() != null) {
2108 final RangeIonosphericDelayModifier ionoModifier = new RangeIonosphericDelayModifier(stationData.getIonosphericModel(),
2109 od.getObservationType().getSignal(system).getFrequency());
2110 range.addModifier(ionoModifier);
2111 }
2112 if (satAntennaRangeModifier != null) {
2113 range.addModifier(satAntennaRangeModifier);
2114 }
2115 if (shapiroRangeModifier != null) {
2116 range.addModifier(shapiroRangeModifier);
2117 }
2118 if (stationData.getRangeBias() != null) {
2119 range.addModifier(stationData.getRangeBias());
2120 }
2121 if (satRangeBias != null) {
2122 range.addModifier(satRangeBias);
2123 }
2124 if (stationData.getRangeTroposphericCorrection() != null) {
2125 range.addModifier(stationData.getRangeTroposphericCorrection());
2126 }
2127 addIfNonZeroWeight(range, measurements);
2128
2129 } else if (od.getObservationType().getMeasurementType() == MeasurementType.DOPPLER && useRangeRateMeasurements) {
2130
2131 final String stationName = rinexObs.getHeader().getMarkerName() + "/" + od.getObservationType();
2132 final StationData stationData = stations.get(stationName);
2133 if (stationData == null) {
2134 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
2135 stationName + notConfigured);
2136 }
2137 final RangeRate rangeRate = new RangeRate(stationData.getStation(), observationDataSet.getDate(),
2138 od.getValue(), stationData.getRangeRateSigma(),
2139 weights.getRangeRateBaseWeight(), false, satellite);
2140 if (stationData.getIonosphericModel() != null) {
2141 final RangeRateIonosphericDelayModifier ionoModifier = new RangeRateIonosphericDelayModifier(stationData.getIonosphericModel(),
2142 od.getObservationType().getSignal(system).getFrequency(),
2143 false);
2144 rangeRate.addModifier(ionoModifier);
2145 }
2146 if (stationData.getRangeRateBias() != null) {
2147 rangeRate.addModifier(stationData.getRangeRateBias());
2148 }
2149 addIfNonZeroWeight(rangeRate, measurements);
2150 }
2151 }
2152 }
2153 }
2154 }
2155
2156 return measurements;
2157
2158 }
2159
2160
2161
2162
2163
2164
2165
2166
2167 private List<ObservedMeasurement<?>> readCpf(final DataSource source,
2168 final ObservableSatellite satellite,
2169 final Orbit initialGuess) {
2170
2171
2172 final CPFParser parserCpf = new CPFParser();
2173 final CPF file = parserCpf.parse(source);
2174
2175
2176 final CPFEphemeris ephemeris = file.getSatellites().get(file.getHeader().getIlrsSatelliteId());
2177 final Frame ephFrame = ephemeris.getFrame();
2178
2179
2180 final List<ObservedMeasurement<?>> measurements = new ArrayList<>();
2181 for (final CPFCoordinate coordinates : ephemeris.getCoordinates()) {
2182 AbsoluteDate date = coordinates.getDate();
2183 final Vector3D posInertial = ephFrame.getStaticTransformTo(initialGuess.getFrame(), date).transformPosition(coordinates.getPosition());
2184 measurements.add(new Position(date, posInertial, 1, 1, satellite));
2185 }
2186
2187
2188 return measurements;
2189 }
2190
2191
2192
2193
2194
2195
2196
2197
2198
2199
2200
2201
2202
2203 private List<ObservedMeasurement<?>> readCrd(final DataSource source,
2204 final Map<String, StationData> stations,
2205 final KeyValueFileParser<ParameterKey> kvParser,
2206 final ObservableSatellite satellite,
2207 final Bias<Range> satRangeBias,
2208 final Weights weights,
2209 final OutlierFilter<Range> rangeOutliersManager,
2210 final ShapiroRangeModifier shapiroRange)
2211 throws IOException {
2212
2213
2214 final List<ObservedMeasurement<?>> measurements = new ArrayList<>();
2215
2216
2217 final CRDParser parser = new CRDParser();
2218 final CRD file = parser.parse(source);
2219
2220
2221 for (final CRDDataBlock block : file.getDataBlocks()) {
2222
2223
2224 final CRDHeader header = block.getHeader();
2225
2226
2227 final double wavelength = block.getConfigurationRecords().getSystemRecord().getWavelength();
2228
2229
2230 final Meteo meteo = block.getMeteoData();
2231
2232
2233 final StationData stationData = stations.get(String.valueOf(header.getSystemIdentifier()));
2234
2235
2236 for (final RangeMeasurement range : block.getRangeData()) {
2237
2238
2239 final double timeOfFlight = range.getTimeOfFlight();
2240
2241
2242 final AbsoluteDate transmitTime = range.getDate();
2243
2244 if (range.getEpochEvent() == 1) {
2245 transmitTime.shiftedBy(-0.5 * timeOfFlight);
2246 }
2247
2248
2249 final AbsoluteDate receivedTime = transmitTime.shiftedBy(timeOfFlight);
2250
2251
2252 boolean twoWays = false;
2253 double rangeValue = timeOfFlight * Constants.SPEED_OF_LIGHT;
2254
2255 if (header.getRangeType() == RangeType.TWO_WAY) {
2256 twoWays = true;
2257 rangeValue = 0.5 * rangeValue;
2258 }
2259
2260
2261 final MeteorologicalMeasurement meteoData = meteo.getMeteo(receivedTime);
2262
2263
2264 final Range measurement = new Range(stationData.getStation(), twoWays, receivedTime, rangeValue,
2265 stationData.getRangeSigma(), weights.getRangeBaseWeight(), satellite);
2266
2267
2268 if (kvParser.containsKey(ParameterKey.RANGE_CENTER_OF_MASS_CORRECTION)) {
2269 final double bias = kvParser.getDouble(ParameterKey.RANGE_CENTER_OF_MASS_CORRECTION);
2270 final Bias<Range> centerOfMass = new Bias<>(new String[] {"center of mass"},
2271 new double[] {bias}, new double[] {1.0}, new double[] {Double.NEGATIVE_INFINITY},
2272 new double[] {Double.POSITIVE_INFINITY});
2273 measurement.addModifier(centerOfMass);
2274 }
2275
2276
2277 final TroposphericModel model;
2278 if (meteoData != null) {
2279 final PressureTemperatureHumidity pth =
2280 new PressureTemperatureHumidity(stationData.getStation().getBaseFrame().getPoint().getAltitude(),
2281 Unit.BAR.toSI(meteoData.getPressure()),
2282 meteoData.getTemperature(),
2283 new CIPM2007().
2284 waterVaporPressure(Unit.BAR.toSI(meteoData.getPressure()),
2285 meteoData.getTemperature(),
2286 0.01 * meteoData.getHumidity()),
2287 Double.NaN, Double.NaN);
2288 model = new MendesPavlisModel(new ConstantPressureTemperatureHumidityProvider(pth),
2289 wavelength, Unit.METRE);
2290 } else {
2291 model = MendesPavlisModel.getStandardModel(wavelength, Unit.METRE);
2292 }
2293 measurement.addModifier(new RangeTroposphericDelayModifier(model));
2294
2295
2296 if (shapiroRange != null) {
2297 measurement.addModifier(shapiroRange);
2298 }
2299
2300
2301 if (stationData.getRangeBias() != null) {
2302 measurement.addModifier(stationData.getRangeBias());
2303 }
2304
2305
2306 if (satRangeBias != null) {
2307 measurement.addModifier(satRangeBias);
2308 }
2309
2310
2311 if (rangeOutliersManager != null) {
2312 measurement.addModifier(rangeOutliersManager);
2313 }
2314
2315 addIfNonZeroWeight(measurement, measurements);
2316 }
2317
2318 }
2319
2320 return measurements;
2321 }
2322
2323
2324
2325
2326
2327
2328 private List<ObservedMeasurement<?>> multiplexMeasurements(final List<ObservedMeasurement<?>> independentMeasurements,
2329 final double tol) {
2330 final List<ObservedMeasurement<?>> multiplexed = new ArrayList<>();
2331 independentMeasurements.sort(new ChronologicalComparator());
2332 List<ObservedMeasurement<?>> clump = new ArrayList<>();
2333 for (final ObservedMeasurement<?> measurement : independentMeasurements) {
2334 if (!clump.isEmpty() && measurement.getDate().durationFrom(clump.get(0).getDate()) > tol) {
2335
2336
2337 if (clump.size() == 1) {
2338 multiplexed.add(clump.get(0));
2339 } else {
2340 multiplexed.add(new MultiplexedMeasurement(clump));
2341 }
2342
2343
2344 clump = new ArrayList<>();
2345
2346 }
2347 clump.add(measurement);
2348 }
2349
2350 if (clump.size() == 1) {
2351 multiplexed.add(clump.get(0));
2352 } else {
2353 multiplexed.add(new MultiplexedMeasurement(clump));
2354 }
2355 return multiplexed;
2356 }
2357
2358
2359
2360
2361 protected void sortParametersChanges(List<? extends ParameterDriver> parameters) {
2362
2363
2364 parameters.sort(new Comparator<ParameterDriver>() {
2365
2366
2367 @Override
2368 public int compare(final ParameterDriver pd1,
2369 final ParameterDriver pd2) {
2370 return pd1.getName().compareTo(pd2.getName());
2371 }
2372
2373 });
2374 }
2375
2376
2377
2378
2379
2380 private static void addIfNonZeroWeight(final ObservedMeasurement<?> measurement, final List<ObservedMeasurement<?>> measurements) {
2381 double sum = 0;
2382 for (double w : measurement.getBaseWeight()) {
2383 sum += FastMath.abs(w);
2384 }
2385 if (sum > Precision.SAFE_MIN) {
2386
2387 measurements.add(measurement);
2388 }
2389 }
2390
2391
2392
2393
2394
2395
2396 private void needsBothOrNeither(final KeyValueFileParser<ParameterKey> parser,
2397 final ParameterKey key1, final ParameterKey key2) {
2398 if (parser.containsKey(key1) != parser.containsKey(key2)) {
2399 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
2400 key1.toString().toLowerCase().replace('_', '.') +
2401 " and " +
2402 key2.toString().toLowerCase().replace('_', '.') +
2403 " must be both present or both absent");
2404 }
2405 }
2406
2407
2408
2409
2410
2411
2412 private void displayParametersChanges(final PrintStream out, final String header, final boolean sort,
2413 final int length, final ParameterDriversList parameters) {
2414
2415 List<ParameterDriver> list = new ArrayList<>(parameters.getDrivers());
2416 if (sort) {
2417
2418 list.sort(new Comparator<ParameterDriver>() {
2419
2420
2421 @Override
2422 public int compare(final ParameterDriver pd1,
2423 final ParameterDriver pd2) {
2424 return pd1.getName().compareTo(pd2.getName());
2425 }
2426
2427 });
2428 }
2429
2430 out.println(header);
2431 int index = 0;
2432 for (final ParameterDriver parameter : list) {
2433 if (parameter.isSelected()) {
2434 final double factor;
2435 if (parameter.getName().endsWith("/az bias") || parameter.getName().endsWith("/el bias")) {
2436 factor = FastMath.toDegrees(1.0);
2437 } else {
2438 factor = 1.0;
2439 }
2440 final double initial = parameter.getReferenceValue();
2441 final double value = parameter.getValue();
2442 out.format(Locale.US, " %2d %s", ++index, parameter.getName());
2443 for (int i = parameter.getName().length(); i < length; ++i) {
2444 out.format(Locale.US, " ");
2445 }
2446 out.format(Locale.US, " %+.12f (final value: % .12f)%n",
2447 factor * (value - initial), factor * value);
2448 }
2449 }
2450
2451 }
2452
2453
2454
2455 private void displayFinalCovariances(final PrintStream logStream, final AbstractKalmanEstimator kalman) {
2456
2457
2458
2459
2460
2461
2462
2463
2464
2465
2466
2467
2468
2469
2470
2471
2472
2473
2474
2475
2476
2477
2478
2479 final RealMatrix P = kalman.getPhysicalEstimatedCovarianceMatrix();
2480 final String[] paramNames = new String[P.getRowDimension()];
2481 int index = 0;
2482 int paramSize = 0;
2483 for (final ParameterDriver driver : kalman.getOrbitalParametersDrivers(true).getDrivers()) {
2484 paramNames[index++] = driver.getName();
2485 paramSize = FastMath.max(paramSize, driver.getName().length());
2486 }
2487 for (final ParameterDriver driver : kalman.getPropagationParametersDrivers(true).getDrivers()) {
2488 paramNames[index++] = driver.getName();
2489 paramSize = FastMath.max(paramSize, driver.getName().length());
2490 }
2491 for (final ParameterDriver driver : kalman.getEstimatedMeasurementsParameters().getDrivers()) {
2492 paramNames[index++] = driver.getName();
2493 paramSize = FastMath.max(paramSize, driver.getName().length());
2494 }
2495 if (paramSize < 20) {
2496 paramSize = 20;
2497 }
2498
2499
2500 logStream.format("\n%s\n", "Kalman Final Covariances:");
2501
2502
2503 logStream.format(Locale.US, "\tDate: %-23s UTC\n",
2504 kalman.getCurrentDate().toString(TimeScalesFactory.getUTC()));
2505
2506
2507 String strFormat = String.format("%%%2ds ", paramSize);
2508 logStream.format(strFormat, "Covariances:");
2509 for (int i = 0; i < P.getRowDimension(); i++) {
2510 logStream.format(Locale.US, strFormat, paramNames[i]);
2511 }
2512 logStream.println();
2513 String numFormat = String.format("%%%2d.6f ", paramSize);
2514 for (int i = 0; i < P.getRowDimension(); i++) {
2515 logStream.format(Locale.US, strFormat, paramNames[i]);
2516 for (int j = 0; j <= i; j++) {
2517 logStream.format(Locale.US, numFormat, P.getEntry(i, j));
2518 }
2519 logStream.println();
2520 }
2521
2522
2523 final double[] sigmas = new double[P.getRowDimension()];
2524 for (int i = 0; i < P.getRowDimension(); i++) {
2525 sigmas[i] = FastMath.sqrt(P.getEntry(i, i));
2526 }
2527
2528 logStream.format("\n" + strFormat, "Corr coef:");
2529 for (int i = 0; i < P.getRowDimension(); i++) {
2530 logStream.format(Locale.US, strFormat, paramNames[i]);
2531 }
2532 logStream.println();
2533 for (int i = 0; i < P.getRowDimension(); i++) {
2534 logStream.format(Locale.US, strFormat, paramNames[i]);
2535 for (int j = 0; j <= i; j++) {
2536 logStream.format(Locale.US, numFormat, P.getEntry(i, j)/(sigmas[i]*sigmas[j]));
2537 }
2538 logStream.println();
2539 }
2540
2541
2542 logStream.format("\n" + strFormat + "\n", "Sigmas: ");
2543 for (int i = 0; i < P.getRowDimension(); i++) {
2544 logStream.format(Locale.US, strFormat + numFormat + "\n", paramNames[i], sigmas[i]);
2545 }
2546 logStream.println();
2547 }
2548
2549
2550
2551 private static void logEvaluation(EstimatedMeasurement<?> evaluation,
2552 EvaluationLogger<Range> rangeLog,
2553 EvaluationLogger<RangeRate> rangeRateLog,
2554 EvaluationLogger<AngularAzEl> azimuthLog,
2555 EvaluationLogger<AngularAzEl> elevationLog,
2556 EvaluationLogger<Position> positionOnlyLog,
2557 EvaluationLogger<PV> positionLog,
2558 EvaluationLogger<PV> velocityLog) {
2559
2560
2561 final String measurementType = evaluation.getObservedMeasurement().getMeasurementType();
2562 switch (measurementType) {
2563 case Range.MEASUREMENT_TYPE: {
2564 @SuppressWarnings("unchecked")
2565 final EstimatedMeasurement<Range> ev = (EstimatedMeasurement<Range>) evaluation;
2566 if (rangeLog != null) {
2567 rangeLog.log(ev);
2568 }
2569 break;
2570 }
2571 case RangeRate.MEASUREMENT_TYPE: {
2572 @SuppressWarnings("unchecked")
2573 final EstimatedMeasurement<RangeRate> ev = (EstimatedMeasurement<RangeRate>) evaluation;
2574 if (rangeRateLog != null) {
2575 rangeRateLog.log(ev);
2576 }
2577 break;
2578 }
2579 case AngularAzEl.MEASUREMENT_TYPE: {
2580 @SuppressWarnings("unchecked")
2581 final EstimatedMeasurement<AngularAzEl> ev = (EstimatedMeasurement<AngularAzEl>) evaluation;
2582 if (azimuthLog != null) {
2583 azimuthLog.log(ev);
2584 }
2585 if (elevationLog != null) {
2586 elevationLog.log(ev);
2587 }
2588 break;
2589 }
2590 case Position.MEASUREMENT_TYPE: {
2591 @SuppressWarnings("unchecked")
2592 final EstimatedMeasurement<Position> ev = (EstimatedMeasurement<Position>) evaluation;
2593 if (positionOnlyLog != null) {
2594 positionOnlyLog.log(ev);
2595 }
2596 break;
2597 }
2598 case PV.MEASUREMENT_TYPE: {
2599 @SuppressWarnings("unchecked")
2600 final EstimatedMeasurement<PV> ev = (EstimatedMeasurement<PV>) evaluation;
2601 if (positionLog != null) {
2602 positionLog.log(ev);
2603 }
2604 if (velocityLog != null) {
2605 velocityLog.log(ev);
2606 }
2607 break;
2608 }
2609 case MultiplexedMeasurement.MEASUREMENT_TYPE:
2610 for (final EstimatedMeasurement<?> em : ((MultiplexedMeasurement) evaluation.getObservedMeasurement()).getEstimatedMeasurements()) {
2611 logEvaluation(em, rangeLog, rangeRateLog, azimuthLog,
2612 elevationLog, positionOnlyLog, positionLog,
2613 velocityLog);
2614 }
2615 break;
2616 }
2617 }
2618
2619
2620 public static class Observer implements KalmanObserver {
2621
2622
2623 private AbsoluteDate t0;
2624
2625
2626 private final Boolean print;
2627
2628
2629 private final RangeLog rangeLog;
2630
2631
2632 private final RangeRateLog rangeRateLog;
2633
2634
2635 private final AzimuthLog azimuthLog;
2636
2637
2638 private final ElevationLog elevationLog;
2639
2640
2641 private final PositionOnlyLog positionOnlyLog;
2642
2643
2644 private final PositionLog positionLog;
2645
2646
2647 private final VelocityLog velocityLog;
2648
2649 public Observer(final Boolean print, final RangeLog rangeLog,
2650 final RangeRateLog rangeRateLog, final AzimuthLog azimuthLog,
2651 final ElevationLog elevationLog, final PositionOnlyLog positionOnlyLog,
2652 final PositionLog positionLog, final VelocityLog velocityLog) {
2653 this.print = print;
2654 this.rangeLog = rangeLog;
2655 this.rangeRateLog = rangeRateLog;
2656 this.azimuthLog = azimuthLog;
2657 this.elevationLog = elevationLog;
2658 this.positionOnlyLog = positionOnlyLog;
2659 this.positionLog = positionLog;
2660 this.velocityLog = velocityLog;
2661 }
2662
2663
2664
2665
2666 @Override
2667 @SuppressWarnings("unchecked")
2668 public void evaluationPerformed(final KalmanEstimation estimation) {
2669
2670
2671 final EstimatedMeasurement<?> estimatedMeasurement = estimation.getCorrectedMeasurement();
2672 final int currentNumber = estimation.getCurrentMeasurementNumber();
2673 final AbsoluteDate currentDate = estimatedMeasurement.getDate();
2674 final EstimatedMeasurement.Status currentStatus = estimatedMeasurement.getStatus();
2675
2676
2677 final ObservedMeasurement<?> observedMeasurement = estimatedMeasurement.getObservedMeasurement();
2678
2679
2680 String measType = "";
2681 String stationName = "";
2682
2683
2684 logEvaluation(estimatedMeasurement,
2685 rangeLog, rangeRateLog, azimuthLog, elevationLog, positionOnlyLog, positionLog, velocityLog);
2686
2687 final String measurementType = observedMeasurement.getMeasurementType();
2688 switch (measurementType) {
2689 case Range.MEASUREMENT_TYPE:
2690 measType = "RANGE";
2691 stationName = ((EstimatedMeasurement<Range>) estimatedMeasurement).
2692 getObservedMeasurement().getStation().getBaseFrame().getName();
2693 break;
2694 case RangeRate.MEASUREMENT_TYPE:
2695 measType = "RANGE_RATE";
2696 stationName = ((EstimatedMeasurement<RangeRate>) estimatedMeasurement).
2697 getObservedMeasurement().getStation().getBaseFrame().getName();
2698 break;
2699 case AngularAzEl.MEASUREMENT_TYPE:
2700 measType = "AZ_EL";
2701 stationName = ((EstimatedMeasurement<AngularAzEl>) estimatedMeasurement).
2702 getObservedMeasurement().getStation().getBaseFrame().getName();
2703 break;
2704 case PV.MEASUREMENT_TYPE:
2705 measType = "PV";
2706 break;
2707 case Position.MEASUREMENT_TYPE:
2708 measType = "POSITION";
2709 break;
2710 }
2711
2712
2713
2714
2715
2716
2717 if (print) {
2718 if (currentNumber == 1) {
2719
2720 t0 = currentDate;
2721
2722
2723 final String formatHeader = "%-4s\t%-25s\t%15s\t%-10s\t%-10s\t%-20s\t%20s\t%20s";
2724 String header = String.format(Locale.US, formatHeader,
2725 "Nb", "Epoch", "Dt[s]", "Status", "Type", "Station",
2726 "DP Corr", "DV Corr");
2727
2728 for (DelegatingDriver driver : estimation.getEstimatedOrbitalParameters().getDrivers()) {
2729 header += String.format(Locale.US, "\t%20s", driver.getName());
2730 header += String.format(Locale.US, "\t%20s", "D" + driver.getName());
2731 }
2732
2733
2734 for (DelegatingDriver driver : estimation.getEstimatedPropagationParameters().getDrivers()) {
2735 header += String.format(Locale.US, "\t%20s", driver.getName());
2736 header += String.format(Locale.US, "\t%20s", "D" + driver.getName());
2737 }
2738
2739
2740 for (DelegatingDriver driver : estimation.getEstimatedMeasurementsParameters().getDrivers()) {
2741 header += String.format(Locale.US, "\t%20s", driver.getName());
2742 header += String.format(Locale.US, "\t%20s", "D" + driver.getName());
2743 }
2744
2745
2746 System.out.println(header);
2747 }
2748
2749
2750 String line = "";
2751
2752 final String lineFormat = "%4d\t%-25s\t%15.3f\t%-10s\t%-10s\t%-20s\t%20.9e\t%20.9e";
2753
2754
2755 final Vector3D predictedP = estimation.getPredictedSpacecraftStates()[0].getPosition();
2756 final Vector3D predictedV = estimation.getPredictedSpacecraftStates()[0].getPVCoordinates().getVelocity();
2757 final Vector3D estimatedP = estimation.getCorrectedSpacecraftStates()[0].getPosition();
2758 final Vector3D estimatedV = estimation.getCorrectedSpacecraftStates()[0].getPVCoordinates().getVelocity();
2759 final double DPcorr = Vector3D.distance(predictedP, estimatedP);
2760 final double DVcorr = Vector3D.distance(predictedV, estimatedV);
2761
2762 line = String.format(Locale.US, lineFormat,
2763 currentNumber, currentDate.toString(),
2764 currentDate.durationFrom(t0), currentStatus.toString(),
2765 measType, stationName,
2766 DPcorr, DVcorr);
2767
2768
2769 int jPar = 0;
2770 final RealMatrix Pest = estimation.getPhysicalEstimatedCovarianceMatrix();
2771
2772 for (DelegatingDriver driver : estimation.getEstimatedOrbitalParameters().getDrivers()) {
2773 line += String.format(Locale.US, "\t%20.9f", driver.getValue());
2774 line += String.format(Locale.US, "\t%20.9e", FastMath.sqrt(Pest.getEntry(jPar, jPar)));
2775 jPar++;
2776 }
2777
2778 for (DelegatingDriver driver : estimation.getEstimatedPropagationParameters().getDrivers()) {
2779 line += String.format(Locale.US, "\t%20.9f", driver.getValue());
2780 line += String.format(Locale.US, "\t%20.9e", FastMath.sqrt(Pest.getEntry(jPar, jPar)));
2781 jPar++;
2782 }
2783
2784 for (DelegatingDriver driver : estimation.getEstimatedMeasurementsParameters().getDrivers()) {
2785 line += String.format(Locale.US, "\t%20.9f", driver.getValue());
2786 line += String.format(Locale.US, "\t%20.9e", FastMath.sqrt(Pest.getEntry(jPar, jPar)));
2787 jPar++;
2788 }
2789
2790
2791 System.out.println(line);
2792 }
2793 }
2794
2795
2796
2797 }
2798
2799 }