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