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