1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.leastsquares;
18
19 import java.io.BufferedReader;
20 import java.io.File;
21 import java.io.FileInputStream;
22 import java.io.IOException;
23 import java.io.InputStreamReader;
24 import java.io.UnsupportedEncodingException;
25 import java.net.URISyntaxException;
26 import java.text.ParseException;
27 import java.util.ArrayList;
28 import java.util.Collections;
29 import java.util.Comparator;
30 import java.util.HashMap;
31 import java.util.List;
32 import java.util.Locale;
33 import java.util.Map;
34 import java.util.NoSuchElementException;
35 import java.util.SortedSet;
36 import java.util.TreeSet;
37 import java.util.regex.Pattern;
38
39 import org.hipparchus.exception.LocalizedCoreFormats;
40 import org.hipparchus.geometry.euclidean.threed.Vector3D;
41 import org.hipparchus.linear.QRDecomposer;
42 import org.hipparchus.linear.RealMatrix;
43 import org.hipparchus.optim.nonlinear.vector.leastsquares.GaussNewtonOptimizer;
44 import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer;
45 import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem;
46 import org.hipparchus.optim.nonlinear.vector.leastsquares.LevenbergMarquardtOptimizer;
47 import org.hipparchus.stat.descriptive.StreamingStatistics;
48 import org.hipparchus.util.FastMath;
49 import org.hipparchus.util.Precision;
50 import org.junit.Assert;
51 import org.junit.Test;
52 import org.orekit.KeyValueFileParser;
53 import org.orekit.Utils;
54 import org.orekit.attitudes.AttitudeProvider;
55 import org.orekit.attitudes.BodyCenterPointing;
56 import org.orekit.attitudes.LofOffset;
57 import org.orekit.attitudes.NadirPointing;
58 import org.orekit.attitudes.YawCompensation;
59 import org.orekit.attitudes.YawSteering;
60 import org.orekit.bodies.CelestialBody;
61 import org.orekit.bodies.CelestialBodyFactory;
62 import org.orekit.bodies.GeodeticPoint;
63 import org.orekit.bodies.OneAxisEllipsoid;
64 import org.orekit.data.DataProvidersManager;
65 import org.orekit.errors.OrekitException;
66 import org.orekit.errors.OrekitMessages;
67 import org.orekit.estimation.measurements.AngularAzEl;
68 import org.orekit.estimation.measurements.EstimatedMeasurement;
69 import org.orekit.estimation.measurements.EstimationsProvider;
70 import org.orekit.estimation.measurements.GroundStation;
71 import org.orekit.estimation.measurements.ObservableSatellite;
72 import org.orekit.estimation.measurements.ObservedMeasurement;
73 import org.orekit.estimation.measurements.PV;
74 import org.orekit.estimation.measurements.Range;
75 import org.orekit.estimation.measurements.RangeRate;
76 import org.orekit.estimation.measurements.modifiers.AngularRadioRefractionModifier;
77 import org.orekit.estimation.measurements.modifiers.Bias;
78 import org.orekit.estimation.measurements.modifiers.OnBoardAntennaRangeModifier;
79 import org.orekit.estimation.measurements.modifiers.OutlierFilter;
80 import org.orekit.estimation.measurements.modifiers.RangeIonosphericDelayModifier;
81 import org.orekit.estimation.measurements.modifiers.RangeRateIonosphericDelayModifier;
82 import org.orekit.estimation.measurements.modifiers.RangeTroposphericDelayModifier;
83 import org.orekit.forces.PolynomialParametricAcceleration;
84 import org.orekit.forces.drag.DragForce;
85 import org.orekit.forces.drag.DragSensitive;
86 import org.orekit.forces.drag.IsotropicDrag;
87 import org.orekit.forces.drag.atmosphere.Atmosphere;
88 import org.orekit.forces.drag.atmosphere.DTM2000;
89 import org.orekit.forces.drag.atmosphere.data.MarshallSolarActivityFutureEstimation;
90 import org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel;
91 import org.orekit.forces.gravity.OceanTides;
92 import org.orekit.forces.gravity.Relativity;
93 import org.orekit.forces.gravity.SolidTides;
94 import org.orekit.forces.gravity.ThirdBodyAttraction;
95 import org.orekit.forces.gravity.potential.GravityFieldFactory;
96 import org.orekit.forces.gravity.potential.ICGEMFormatReader;
97 import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider;
98 import org.orekit.forces.radiation.IsotropicRadiationSingleCoefficient;
99 import org.orekit.forces.radiation.RadiationSensitive;
100 import org.orekit.forces.radiation.SolarRadiationPressure;
101 import org.orekit.frames.EOPHistory;
102 import org.orekit.frames.Frame;
103 import org.orekit.frames.FramesFactory;
104 import org.orekit.frames.LOFType;
105 import org.orekit.frames.TopocentricFrame;
106 import org.orekit.frames.Transform;
107 import org.orekit.gnss.Frequency;
108 import org.orekit.gnss.MeasurementType;
109 import org.orekit.gnss.ObservationData;
110 import org.orekit.gnss.ObservationDataSet;
111 import org.orekit.gnss.RinexLoader;
112 import org.orekit.gnss.SatelliteSystem;
113 import org.orekit.models.AtmosphericRefractionModel;
114 import org.orekit.models.earth.DiscreteTroposphericModel;
115 import org.orekit.models.earth.EarthITU453AtmosphereRefraction;
116 import org.orekit.models.earth.EstimatedTroposphericModel;
117 import org.orekit.models.earth.GlobalMappingFunctionModel;
118 import org.orekit.models.earth.GlobalPressureTemperatureModel;
119 import org.orekit.models.earth.IonosphericModel;
120 import org.orekit.models.earth.KlobucharIonoCoefficientsLoader;
121 import org.orekit.models.earth.KlobucharIonoModel;
122 import org.orekit.models.earth.MappingFunction;
123 import org.orekit.models.earth.NiellMappingFunctionModel;
124 import org.orekit.models.earth.SaastamoinenModel;
125 import org.orekit.models.earth.displacement.OceanLoading;
126 import org.orekit.models.earth.displacement.OceanLoadingCoefficientsBLQFactory;
127 import org.orekit.models.earth.displacement.StationDisplacement;
128 import org.orekit.models.earth.displacement.TidalDisplacement;
129 import org.orekit.orbits.CartesianOrbit;
130 import org.orekit.orbits.CircularOrbit;
131 import org.orekit.orbits.EquinoctialOrbit;
132 import org.orekit.orbits.KeplerianOrbit;
133 import org.orekit.orbits.Orbit;
134 import org.orekit.orbits.PositionAngle;
135 import org.orekit.propagation.SpacecraftState;
136 import org.orekit.propagation.analytical.tle.TLE;
137 import org.orekit.propagation.analytical.tle.TLEPropagator;
138 import org.orekit.propagation.conversion.DormandPrince853IntegratorBuilder;
139 import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
140 import org.orekit.time.AbsoluteDate;
141 import org.orekit.time.DateComponents;
142 import org.orekit.time.TimeScalesFactory;
143 import org.orekit.utils.Constants;
144 import org.orekit.utils.IERSConventions;
145 import org.orekit.utils.PVCoordinates;
146 import org.orekit.utils.ParameterDriver;
147 import org.orekit.utils.ParameterDriversList;
148 import org.orekit.utils.ParameterDriversList.DelegatingDriver;
149 import org.orekit.utils.TimeStampedPVCoordinates;
150
151 public class OrbitDeterminationTest {
152
153 @Test
154
155 public void testLageos2()
156 throws URISyntaxException, IllegalArgumentException, IOException,
157 OrekitException, ParseException {
158
159
160 final String inputPath = OrbitDeterminationTest.class.getClassLoader().getResource("orbit-determination/Lageos2/od_test_Lageos2.in").toURI().getPath();
161 final File input = new File(inputPath);
162
163
164 Utils.setDataRoot("orbit-determination/february-2016:potential/icgem-format");
165 GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-6s-truncated", true));
166
167
168 ResultOD odLageos2 = run(input, false);
169
170
171
172 final double distanceAccuracy = 0.1;
173 final double velocityAccuracy = 1e-4;
174
175
176 final int numberOfIte = 4;
177 final int numberOfEval = 4;
178
179 Assert.assertEquals(numberOfIte, odLageos2.getNumberOfIteration());
180 Assert.assertEquals(numberOfEval, odLageos2.getNumberOfEvaluation());
181
182
183 final Vector3D estimatedPos = odLageos2.getEstimatedPV().getPosition();
184 final Vector3D estimatedVel = odLageos2.getEstimatedPV().getVelocity();
185
186
187 final Vector3D refPos = new Vector3D(-5532131.956902, 10025696.592156, -3578940.040009);
188 final Vector3D refVel = new Vector3D(-3871.275109, -607.880985, 4280.972530);
189 Assert.assertEquals(0.0, Vector3D.distance(refPos, estimatedPos), distanceAccuracy);
190 Assert.assertEquals(0.0, Vector3D.distance(refVel, estimatedVel), velocityAccuracy);
191
192
193 final List<DelegatingDriver> list = new ArrayList<DelegatingDriver>();
194 list.addAll(odLageos2.measurementsParameters.getDrivers());
195 sortParametersChanges(list);
196
197
198 final double[] stationOffSet = { 1.659203, 0.861250, -0.885352 };
199 final double rangeBias = -0.286275;
200 Assert.assertEquals(stationOffSet[0], list.get(0).getValue(), distanceAccuracy);
201 Assert.assertEquals(stationOffSet[1], list.get(1).getValue(), distanceAccuracy);
202 Assert.assertEquals(stationOffSet[2], list.get(2).getValue(), distanceAccuracy);
203 Assert.assertEquals(rangeBias, list.get(3).getValue(), distanceAccuracy);
204
205
206 final long nbRange = 258;
207
208 final double[] RefStatRange = { -2.431135, 2.218644, 0.038483, 0.982017 };
209 Assert.assertEquals(nbRange, odLageos2.getRangeStat().getN());
210 Assert.assertEquals(RefStatRange[0], odLageos2.getRangeStat().getMin(), distanceAccuracy);
211 Assert.assertEquals(RefStatRange[1], odLageos2.getRangeStat().getMax(), distanceAccuracy);
212 Assert.assertEquals(RefStatRange[2], odLageos2.getRangeStat().getMean(), distanceAccuracy);
213 Assert.assertEquals(RefStatRange[3], odLageos2.getRangeStat().getStandardDeviation(), distanceAccuracy);
214
215 }
216
217 @Test
218
219 public void testGNSS()
220 throws URISyntaxException, IllegalArgumentException, IOException,
221 OrekitException, ParseException {
222
223
224 final String inputPath = OrbitDeterminationTest.class.getClassLoader().getResource("orbit-determination/GNSS/od_test_GPS07.in").toURI().getPath();
225 final File input = new File(inputPath);
226
227
228 Utils.setDataRoot("orbit-determination/february-2016:potential/icgem-format");
229 GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-6s-truncated", true));
230
231
232 ResultOD odGNSS = run(input, false);
233
234
235
236
237 final double distanceAccuracy = 11.5;
238 final double velocityAccuracy = 4.0e-3;
239
240
241 final int numberOfIte = 3;
242 final int numberOfEval = 5;
243
244 Assert.assertEquals(numberOfIte, odGNSS.getNumberOfIteration());
245 Assert.assertEquals(numberOfEval, odGNSS.getNumberOfEvaluation());
246
247
248 final Vector3D estimatedPos = odGNSS.getEstimatedPV().getPosition();
249 final Vector3D estimatedVel = odGNSS.getEstimatedPV().getVelocity();
250 final Vector3D refPos = new Vector3D(-2747606.680868164, 22572091.30648564, 13522761.402325712);
251 final Vector3D refVel = new Vector3D(-2729.5151218788005, 1142.6629459030657, -2523.9055974487947);
252 Assert.assertEquals(0.0, Vector3D.distance(refPos, estimatedPos), distanceAccuracy);
253 Assert.assertEquals(0.0, Vector3D.distance(refVel, estimatedVel), velocityAccuracy);
254
255
256 final long nbRange = 4009;
257 final double[] RefStatRange = { -2.706, 2.566, 0.0, 0.738 };
258 Assert.assertEquals(nbRange, odGNSS.getRangeStat().getN());
259 Assert.assertEquals(RefStatRange[0], odGNSS.getRangeStat().getMin(), 0.3);
260 Assert.assertEquals(RefStatRange[1], odGNSS.getRangeStat().getMax(), 0.3);
261 Assert.assertEquals(RefStatRange[2], odGNSS.getRangeStat().getMean(), 1.0e-3);
262 Assert.assertEquals(RefStatRange[3], odGNSS.getRangeStat().getStandardDeviation(), 0.3);
263
264 }
265
266 @Test
267
268 public void testW3B()
269 throws URISyntaxException, IllegalArgumentException, IOException,
270 OrekitException, ParseException {
271
272
273 final String inputPath = OrbitDeterminationTest.class.getClassLoader().getResource("orbit-determination/W3B/od_test_W3.in").toURI().getPath();
274 final File input = new File(inputPath);
275
276
277 Utils.setDataRoot("orbit-determination/W3B:potential/icgem-format");
278 GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-6s-truncated", true));
279
280
281 ResultOD odsatW3 = run(input, false);
282
283
284
285 final double distanceAccuracy = 0.1;
286 final double velocityAccuracy = 1e-4;
287 final double angleAccuracy = 1e-5;
288
289
290 Assert.assertTrue(odsatW3.getNumberOfIteration() < 6);
291 Assert.assertTrue(odsatW3.getNumberOfEvaluation() < 10);
292
293
294 final Vector3D estimatedPos = odsatW3.getEstimatedPV().getPosition();
295 final Vector3D estimatedVel = odsatW3.getEstimatedPV().getVelocity();
296 final Vector3D refPos = new Vector3D(-40541446.255, -9905357.41, 206777.413);
297 final Vector3D refVel = new Vector3D(759.0685, -1476.5156, 54.793);
298 Assert.assertEquals(0.0, Vector3D.distance(refPos, estimatedPos), distanceAccuracy);
299 Assert.assertEquals(0.0, Vector3D.distance(refVel, estimatedVel), velocityAccuracy);
300
301
302
303 final double dragCoef = -0.2154;
304 Assert.assertEquals(dragCoef, odsatW3.propagatorParameters.getDrivers().get(0).getValue(), 1e-3);
305 final Vector3D leakAcceleration0 =
306 new Vector3D(odsatW3.propagatorParameters.getDrivers().get(1).getValue(),
307 odsatW3.propagatorParameters.getDrivers().get(3).getValue(),
308 odsatW3.propagatorParameters.getDrivers().get(5).getValue());
309
310 Assert.assertEquals(8.002e-6, leakAcceleration0.getNorm(), 1.0e-8);
311 final Vector3D leakAcceleration1 =
312 new Vector3D(odsatW3.propagatorParameters.getDrivers().get(2).getValue(),
313 odsatW3.propagatorParameters.getDrivers().get(4).getValue(),
314 odsatW3.propagatorParameters.getDrivers().get(6).getValue());
315 Assert.assertEquals(3.058e-10, leakAcceleration1.getNorm(), 1.0e-12);
316
317
318 final List<DelegatingDriver> list = new ArrayList<DelegatingDriver>();
319 list.addAll(odsatW3.measurementsParameters.getDrivers());
320 sortParametersChanges(list);
321
322
323 final double[] CastleAzElBias = { 0.062701342, -0.003613508 };
324 final double CastleRangeBias = 11274.4677;
325 Assert.assertEquals(CastleAzElBias[0], FastMath.toDegrees(list.get(0).getValue()), angleAccuracy);
326 Assert.assertEquals(CastleAzElBias[1], FastMath.toDegrees(list.get(1).getValue()), angleAccuracy);
327 Assert.assertEquals(CastleRangeBias, list.get(2).getValue(), distanceAccuracy);
328
329
330 final double[] FucAzElBias = { -0.053526137, 0.075483886 };
331 final double FucRangeBias = 13467.8256;
332 Assert.assertEquals(FucAzElBias[0], FastMath.toDegrees(list.get(3).getValue()), angleAccuracy);
333 Assert.assertEquals(FucAzElBias[1], FastMath.toDegrees(list.get(4).getValue()), angleAccuracy);
334 Assert.assertEquals(FucRangeBias, list.get(5).getValue(), distanceAccuracy);
335
336
337 final double[] KumAzElBias = { -0.023574208, -0.054520756 };
338 final double KumRangeBias = 13512.57594;
339 Assert.assertEquals(KumAzElBias[0], FastMath.toDegrees(list.get(6).getValue()), angleAccuracy);
340 Assert.assertEquals(KumAzElBias[1], FastMath.toDegrees(list.get(7).getValue()), angleAccuracy);
341 Assert.assertEquals(KumRangeBias, list.get(8).getValue(), distanceAccuracy);
342
343
344 final double[] PreAzElBias = { 0.030201539, 0.009747877 };
345 final double PreRangeBias = 13594.11889;
346 Assert.assertEquals(PreAzElBias[0], FastMath.toDegrees(list.get( 9).getValue()), angleAccuracy);
347 Assert.assertEquals(PreAzElBias[1], FastMath.toDegrees(list.get(10).getValue()), angleAccuracy);
348 Assert.assertEquals(PreRangeBias, list.get(11).getValue(), distanceAccuracy);
349
350
351 final double[] UraAzElBias = { 0.167814449, -0.12305252 };
352 final double UraRangeBias = 13450.26738;
353 Assert.assertEquals(UraAzElBias[0], FastMath.toDegrees(list.get(12).getValue()), angleAccuracy);
354 Assert.assertEquals(UraAzElBias[1], FastMath.toDegrees(list.get(13).getValue()), angleAccuracy);
355 Assert.assertEquals(UraRangeBias, list.get(14).getValue(), distanceAccuracy);
356
357
358 final long nbRange = 182;
359
360 final double[] RefStatRange = { -18.39149369, 12.54165259, -4.32E-05, 4.374712716 };
361 Assert.assertEquals(nbRange, odsatW3.getRangeStat().getN());
362 Assert.assertEquals(RefStatRange[0], odsatW3.getRangeStat().getMin(), distanceAccuracy);
363 Assert.assertEquals(RefStatRange[1], odsatW3.getRangeStat().getMax(), distanceAccuracy);
364 Assert.assertEquals(RefStatRange[2], odsatW3.getRangeStat().getMean(), distanceAccuracy);
365 Assert.assertEquals(RefStatRange[3], odsatW3.getRangeStat().getStandardDeviation(), distanceAccuracy);
366
367
368 final long nbAzi = 339;
369
370 final double[] RefStatAzi = { -0.043033616, 0.025297558, -1.39E-10, 0.010063041 };
371 Assert.assertEquals(nbAzi, odsatW3.getAzimStat().getN());
372 Assert.assertEquals(RefStatAzi[0], odsatW3.getAzimStat().getMin(), angleAccuracy);
373 Assert.assertEquals(RefStatAzi[1], odsatW3.getAzimStat().getMax(), angleAccuracy);
374 Assert.assertEquals(RefStatAzi[2], odsatW3.getAzimStat().getMean(), angleAccuracy);
375 Assert.assertEquals(RefStatAzi[3], odsatW3.getAzimStat().getStandardDeviation(), angleAccuracy);
376
377
378 final long nbEle = 339;
379 final double[] RefStatEle = { -0.025061971, 0.056294405, -4.10E-11, 0.011604931 };
380 Assert.assertEquals(nbEle, odsatW3.getElevStat().getN());
381 Assert.assertEquals(RefStatEle[0], odsatW3.getElevStat().getMin(), angleAccuracy);
382 Assert.assertEquals(RefStatEle[1], odsatW3.getElevStat().getMax(), angleAccuracy);
383 Assert.assertEquals(RefStatEle[2], odsatW3.getElevStat().getMean(), angleAccuracy);
384 Assert.assertEquals(RefStatEle[3], odsatW3.getElevStat().getStandardDeviation(), angleAccuracy);
385
386 RealMatrix covariances = odsatW3.getCovariances();
387 Assert.assertEquals(28, covariances.getRowDimension());
388 Assert.assertEquals(28, covariances.getColumnDimension());
389
390
391 Assert.assertEquals(0.687998, covariances.getEntry(6, 6), 1.0e-4);
392
393
394 Assert.assertEquals(2.0540e-12, covariances.getEntry(7, 7), 1.0e-16);
395
396
397 Assert.assertEquals(2.4930e-11, covariances.getEntry(9, 9), 1.0e-15);
398
399
400 Assert.assertEquals(7.6720e-11, covariances.getEntry(11, 11), 1.0e-15);
401 }
402
403 private class ResultOD {
404 private int numberOfIteration;
405 private int numberOfEvaluation;
406 private TimeStampedPVCoordinates estimatedPV;
407 private StreamingStatistics rangeStat;
408 private StreamingStatistics azimStat;
409 private StreamingStatistics elevStat;
410 private ParameterDriversList propagatorParameters ;
411 private ParameterDriversList measurementsParameters;
412 private RealMatrix covariances;
413 ResultOD(ParameterDriversList propagatorParameters,
414 ParameterDriversList measurementsParameters,
415 int numberOfIteration, int numberOfEvaluation, TimeStampedPVCoordinates estimatedPV,
416 StreamingStatistics rangeStat, StreamingStatistics rangeRateStat,
417 StreamingStatistics azimStat, StreamingStatistics elevStat,
418 StreamingStatistics posStat, StreamingStatistics velStat,
419 RealMatrix covariances) {
420
421 this.propagatorParameters = propagatorParameters;
422 this.measurementsParameters = measurementsParameters;
423 this.numberOfIteration = numberOfIteration;
424 this.numberOfEvaluation = numberOfEvaluation;
425 this.estimatedPV = estimatedPV;
426 this.rangeStat = rangeStat;
427 this.azimStat = azimStat;
428 this.elevStat = elevStat;
429 this.covariances = covariances;
430 }
431
432 public int getNumberOfIteration() {
433 return numberOfIteration;
434 }
435
436 public int getNumberOfEvaluation() {
437 return numberOfEvaluation;
438 }
439
440 public PVCoordinates getEstimatedPV() {
441 return estimatedPV;
442 }
443
444 public StreamingStatistics getRangeStat() {
445 return rangeStat;
446 }
447
448 public StreamingStatistics getAzimStat() {
449 return azimStat;
450 }
451
452 public StreamingStatistics getElevStat() {
453 return elevStat;
454 }
455
456 public RealMatrix getCovariances() {
457 return covariances;
458 }
459
460 }
461
462 private ResultOD run(final File input, final boolean print)
463 throws IOException, IllegalArgumentException, OrekitException, ParseException {
464
465
466 KeyValueFileParser<ParameterKey> parser = new KeyValueFileParser<ParameterKey>(ParameterKey.class);
467 parser.parseInput(input.getAbsolutePath(), new FileInputStream(input));
468
469
470
471 final RangeLog rangeLog = new RangeLog();
472 final RangeRateLog rangeRateLog = new RangeRateLog();
473 final AzimuthLog azimuthLog = new AzimuthLog();
474 final ElevationLog elevationLog = new ElevationLog();
475 final PositionLog positionLog = new PositionLog();
476 final VelocityLog velocityLog = new VelocityLog();
477
478
479 GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-5c.gfc", true));
480 final NormalizedSphericalHarmonicsProvider gravityField = createGravityField(parser);
481
482
483 final Orbit initialGuess = createOrbit(parser, gravityField.getMu());
484
485
486 final IERSConventions conventions;
487 if (!parser.containsKey(ParameterKey.IERS_CONVENTIONS)) {
488 conventions = IERSConventions.IERS_2010;
489 } else {
490 conventions = IERSConventions.valueOf("IERS_" + parser.getInt(ParameterKey.IERS_CONVENTIONS));
491 }
492
493
494 final OneAxisEllipsoid body = createBody(parser);
495
496
497 final NumericalPropagatorBuilder propagatorBuilder =
498 createPropagatorBuilder(parser, conventions, gravityField, body, initialGuess);
499
500
501 final BatchLSEstimator estimator = createEstimator(parser, propagatorBuilder);
502
503 final Map<String, StationData> stations = createStationsData(parser, conventions, body);
504 final PVData pvData = createPVData(parser);
505 final ObservableSatellite satellite = createObservableSatellite(parser);
506 final Bias<Range> satRangeBias = createSatRangeBias(parser);
507 final OnBoardAntennaRangeModifier satAntennaRangeModifier = createSatAntennaRangeModifier(parser);
508 final Weights weights = createWeights(parser);
509 final OutlierFilter<Range> rangeOutliersManager = createRangeOutliersManager(parser);
510 final OutlierFilter<RangeRate> rangeRateOutliersManager = createRangeRateOutliersManager(parser);
511 final OutlierFilter<AngularAzEl> azElOutliersManager = createAzElOutliersManager(parser);
512 final OutlierFilter<PV> pvOutliersManager = createPVOutliersManager(parser);
513
514
515 final List<ObservedMeasurement<?>> measurements = new ArrayList<ObservedMeasurement<?>>();
516 for (final String fileName : parser.getStringsList(ParameterKey.MEASUREMENTS_FILES, ',')) {
517 if (Pattern.matches(RinexLoader.DEFAULT_RINEX_2_SUPPORTED_NAMES, fileName) ||
518 Pattern.matches(RinexLoader.DEFAULT_RINEX_3_SUPPORTED_NAMES, fileName)) {
519
520 measurements.addAll(readRinex(new File(input.getParentFile(), fileName),
521 parser.getString(ParameterKey.SATELLITE_ID_IN_RINEX_FILES),
522 stations, satellite, satRangeBias, satAntennaRangeModifier, weights,
523 rangeOutliersManager, rangeRateOutliersManager));
524 } else {
525
526 measurements.addAll(readMeasurements(new File(input.getParentFile(), fileName),
527 stations, pvData, satellite,
528 satRangeBias, satAntennaRangeModifier, weights,
529 rangeOutliersManager,
530 rangeRateOutliersManager,
531 azElOutliersManager,
532 pvOutliersManager));
533 }
534
535 }
536
537 for (ObservedMeasurement<?> measurement : measurements) {
538 estimator.addMeasurement(measurement);
539 }
540
541 if (print) {
542 estimator.setObserver(new BatchLSObserver() {
543
544 private PVCoordinates previousPV;
545 {
546 previousPV = initialGuess.getPVCoordinates();
547 final String header = "iteration evaluations ΔP(m) ΔV(m/s) RMS nb Range nb Range-rate nb Angular nb PV%n";
548 System.out.format(Locale.US, header);
549 }
550
551
552 @Override
553 public void evaluationPerformed(final int iterationsCount, final int evaluationsCount,
554 final Orbit[] orbits,
555 final ParameterDriversList estimatedOrbitalParameters,
556 final ParameterDriversList estimatedPropagatorParameters,
557 final ParameterDriversList estimatedMeasurementsParameters,
558 final EstimationsProvider evaluationsProvider,
559 final LeastSquaresProblem.Evaluation lspEvaluation) {
560 PVCoordinates currentPV = orbits[0].getPVCoordinates();
561 final String format0 = " %2d %2d %16.12f %s %s %s %s%n";
562 final String format = " %2d %2d %13.6f %12.9f %16.12f %s %s %s %s%n";
563 final EvaluationCounter<Range> rangeCounter = new EvaluationCounter<Range>();
564 final EvaluationCounter<RangeRate> rangeRateCounter = new EvaluationCounter<RangeRate>();
565 final EvaluationCounter<AngularAzEl> angularCounter = new EvaluationCounter<AngularAzEl>();
566 final EvaluationCounter<PV> pvCounter = new EvaluationCounter<PV>();
567 for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry : estimator.getLastEstimations().entrySet()) {
568 if (entry.getKey() instanceof Range) {
569 @SuppressWarnings("unchecked")
570 EstimatedMeasurement<Range> evaluation = (EstimatedMeasurement<Range>) entry.getValue();
571 rangeCounter.add(evaluation);
572 } else if (entry.getKey() instanceof RangeRate) {
573 @SuppressWarnings("unchecked")
574 EstimatedMeasurement<RangeRate> evaluation = (EstimatedMeasurement<RangeRate>) entry.getValue();
575 rangeRateCounter.add(evaluation);
576 } else if (entry.getKey() instanceof AngularAzEl) {
577 @SuppressWarnings("unchecked")
578 EstimatedMeasurement<AngularAzEl> evaluation = (EstimatedMeasurement<AngularAzEl>) entry.getValue();
579 angularCounter.add(evaluation);
580 } else if (entry.getKey() instanceof PV) {
581 @SuppressWarnings("unchecked")
582 EstimatedMeasurement<PV> evaluation = (EstimatedMeasurement<PV>) entry.getValue();
583 pvCounter.add(evaluation);
584 }
585 }
586 if (evaluationsCount == 1) {
587 System.out.format(Locale.US, format0,
588 iterationsCount, evaluationsCount,
589 lspEvaluation.getRMS(),
590 rangeCounter.format(8), rangeRateCounter.format(8),
591 angularCounter.format(8), pvCounter.format(8));
592 } else {
593 System.out.format(Locale.US, format,
594 iterationsCount, evaluationsCount,
595 Vector3D.distance(previousPV.getPosition(), currentPV.getPosition()),
596 Vector3D.distance(previousPV.getVelocity(), currentPV.getVelocity()),
597 lspEvaluation.getRMS(),
598 rangeCounter.format(8), rangeRateCounter.format(8),
599 angularCounter.format(8), pvCounter.format(8));
600 }
601 previousPV = currentPV;
602 }
603 });
604 }
605 Orbit estimated = estimator.estimate()[0].getInitialState().getOrbit();
606
607
608 for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry : estimator.getLastEstimations().entrySet()) {
609 if (entry.getKey() instanceof Range) {
610 @SuppressWarnings("unchecked")
611 EstimatedMeasurement<Range> evaluation = (EstimatedMeasurement<Range>) entry.getValue();
612 rangeLog.add(evaluation);
613 } else if (entry.getKey() instanceof RangeRate) {
614 @SuppressWarnings("unchecked")
615 EstimatedMeasurement<RangeRate> evaluation = (EstimatedMeasurement<RangeRate>) entry.getValue();
616 rangeRateLog.add(evaluation);
617 } else if (entry.getKey() instanceof AngularAzEl) {
618 @SuppressWarnings("unchecked")
619 EstimatedMeasurement<AngularAzEl> evaluation = (EstimatedMeasurement<AngularAzEl>) entry.getValue();
620 azimuthLog.add(evaluation);
621 elevationLog.add(evaluation);
622 } else if (entry.getKey() instanceof PV) {
623 @SuppressWarnings("unchecked")
624 EstimatedMeasurement<PV> evaluation = (EstimatedMeasurement<PV>) entry.getValue();
625 positionLog.add(evaluation);
626 velocityLog.add(evaluation);
627 }
628 }
629
630
631 final ParameterDriversList propagatorParameters = estimator.getPropagatorParametersDrivers(true);
632 final ParameterDriversList measurementsParameters = estimator.getMeasurementsParametersDrivers(true);
633
634
635 return new ResultOD(propagatorParameters, measurementsParameters,
636 estimator.getIterationsCount(), estimator.getEvaluationsCount(), estimated.getPVCoordinates(),
637 rangeLog.createStatisticsSummary(), rangeRateLog.createStatisticsSummary(),
638 azimuthLog.createStatisticsSummary(), elevationLog.createStatisticsSummary(),
639 positionLog.createStatisticsSummary(), velocityLog.createStatisticsSummary(),
640 estimator.getPhysicalCovariances(1.0e-10));
641 }
642
643
644
645
646 private void sortParametersChanges(List<? extends ParameterDriver> parameters) {
647
648
649 Collections.sort(parameters, new Comparator<ParameterDriver>() {
650
651 @Override
652 public int compare(final ParameterDriver pd1, final ParameterDriver pd2) {
653 return pd1.getName().compareTo(pd2.getName());
654 }
655
656 });
657 }
658
659
660
661
662
663
664
665
666
667
668
669 private NumericalPropagatorBuilder createPropagatorBuilder(final KeyValueFileParser<ParameterKey> parser,
670 final IERSConventions conventions,
671 final NormalizedSphericalHarmonicsProvider gravityField,
672 final OneAxisEllipsoid body,
673 final Orbit orbit)
674 throws NoSuchElementException {
675
676 final double minStep;
677 if (!parser.containsKey(ParameterKey.PROPAGATOR_MIN_STEP)) {
678 minStep = 0.001;
679 } else {
680 minStep = parser.getDouble(ParameterKey.PROPAGATOR_MIN_STEP);
681 }
682
683 final double maxStep;
684 if (!parser.containsKey(ParameterKey.PROPAGATOR_MAX_STEP)) {
685 maxStep = 300;
686 } else {
687 maxStep = parser.getDouble(ParameterKey.PROPAGATOR_MAX_STEP);
688 }
689
690 final double dP;
691 if (!parser.containsKey(ParameterKey.PROPAGATOR_POSITION_ERROR)) {
692 dP = 10.0;
693 } else {
694 dP = parser.getDouble(ParameterKey.PROPAGATOR_POSITION_ERROR);
695 }
696
697 final double positionScale;
698 if (!parser.containsKey(ParameterKey.ESTIMATOR_ORBITAL_PARAMETERS_POSITION_SCALE)) {
699 positionScale = dP;
700 } else {
701 positionScale = parser.getDouble(ParameterKey.ESTIMATOR_ORBITAL_PARAMETERS_POSITION_SCALE);
702 }
703 final NumericalPropagatorBuilder propagatorBuilder =
704 new NumericalPropagatorBuilder(orbit,
705 new DormandPrince853IntegratorBuilder(minStep, maxStep, dP),
706 PositionAngle.MEAN,
707 positionScale);
708
709
710 final double mass;
711 if (!parser.containsKey(ParameterKey.MASS)) {
712 mass = 1000.0;
713 } else {
714 mass = parser.getDouble(ParameterKey.MASS);
715 }
716 propagatorBuilder.setMass(mass);
717
718
719 propagatorBuilder.addForceModel(new HolmesFeatherstoneAttractionModel(body.getBodyFrame(), gravityField));
720
721
722 if (parser.containsKey(ParameterKey.OCEAN_TIDES_DEGREE) &&
723 parser.containsKey(ParameterKey.OCEAN_TIDES_ORDER)) {
724 final int degree = parser.getInt(ParameterKey.OCEAN_TIDES_DEGREE);
725 final int order = parser.getInt(ParameterKey.OCEAN_TIDES_ORDER);
726 if (degree > 0 && order > 0) {
727 propagatorBuilder.addForceModel(new OceanTides(body.getBodyFrame(),
728 gravityField.getAe(), gravityField.getMu(),
729 degree, order, conventions,
730 TimeScalesFactory.getUT1(conventions, true)));
731 }
732 }
733
734
735 List<CelestialBody> solidTidesBodies = new ArrayList<CelestialBody>();
736 if (parser.containsKey(ParameterKey.SOLID_TIDES_SUN) &&
737 parser.getBoolean(ParameterKey.SOLID_TIDES_SUN)) {
738 solidTidesBodies.add(CelestialBodyFactory.getSun());
739 }
740 if (parser.containsKey(ParameterKey.SOLID_TIDES_MOON) &&
741 parser.getBoolean(ParameterKey.SOLID_TIDES_MOON)) {
742 solidTidesBodies.add(CelestialBodyFactory.getMoon());
743 }
744 if (!solidTidesBodies.isEmpty()) {
745 propagatorBuilder.addForceModel(new SolidTides(body.getBodyFrame(),
746 gravityField.getAe(), gravityField.getMu(),
747 gravityField.getTideSystem(), conventions,
748 TimeScalesFactory.getUT1(conventions, true),
749 solidTidesBodies.toArray(new CelestialBody[solidTidesBodies.size()])));
750 }
751
752
753 if (parser.containsKey(ParameterKey.THIRD_BODY_SUN) &&
754 parser.getBoolean(ParameterKey.THIRD_BODY_SUN)) {
755 propagatorBuilder.addForceModel(new ThirdBodyAttraction(CelestialBodyFactory.getSun()));
756 }
757 if (parser.containsKey(ParameterKey.THIRD_BODY_MOON) &&
758 parser.getBoolean(ParameterKey.THIRD_BODY_MOON)) {
759 propagatorBuilder.addForceModel(new ThirdBodyAttraction(CelestialBodyFactory.getMoon()));
760 }
761
762
763 if (parser.containsKey(ParameterKey.DRAG) && parser.getBoolean(ParameterKey.DRAG)) {
764 final double cd = parser.getDouble(ParameterKey.DRAG_CD);
765 final double area = parser.getDouble(ParameterKey.DRAG_AREA);
766 final boolean cdEstimated = parser.getBoolean(ParameterKey.DRAG_CD_ESTIMATED);
767
768 MarshallSolarActivityFutureEstimation msafe =
769 new MarshallSolarActivityFutureEstimation("(?:Jan|Feb|Mar|Apr|May|Jun|Jul|Aug|Sep|Oct|Nov|Dec)\\p{Digit}\\p{Digit}\\p{Digit}\\p{Digit}F10\\.(?:txt|TXT)",
770 MarshallSolarActivityFutureEstimation.StrengthLevel.AVERAGE);
771 DataProvidersManager manager = DataProvidersManager.getInstance();
772 manager.feed(msafe.getSupportedNames(), msafe);
773 Atmosphere atmosphere = new DTM2000(msafe, CelestialBodyFactory.getSun(), body);
774 propagatorBuilder.addForceModel(new DragForce(atmosphere, new IsotropicDrag(area, cd)));
775 if (cdEstimated) {
776 for (final ParameterDriver driver : propagatorBuilder.getPropagationParametersDrivers().getDrivers()) {
777 if (driver.getName().equals(DragSensitive.DRAG_COEFFICIENT)) {
778 driver.setSelected(true);
779 }
780 }
781 }
782 }
783
784
785 if (parser.containsKey(ParameterKey.SOLAR_RADIATION_PRESSURE) && parser.getBoolean(ParameterKey.SOLAR_RADIATION_PRESSURE)) {
786 final double cr = parser.getDouble(ParameterKey.SOLAR_RADIATION_PRESSURE_CR);
787 final double area = parser.getDouble(ParameterKey.SOLAR_RADIATION_PRESSURE_AREA);
788 final boolean cREstimated = parser.getBoolean(ParameterKey.SOLAR_RADIATION_PRESSURE_CR_ESTIMATED);
789
790 propagatorBuilder.addForceModel(new SolarRadiationPressure(CelestialBodyFactory.getSun(),
791 body.getEquatorialRadius(),
792 new IsotropicRadiationSingleCoefficient(area, cr)));
793 if (cREstimated) {
794 for (final ParameterDriver driver : propagatorBuilder.getPropagationParametersDrivers().getDrivers()) {
795 if (driver.getName().equals(RadiationSensitive.REFLECTION_COEFFICIENT)) {
796 driver.setSelected(true);
797 }
798 }
799 }
800 }
801
802
803 if (parser.containsKey(ParameterKey.GENERAL_RELATIVITY) && parser.getBoolean(ParameterKey.GENERAL_RELATIVITY)) {
804 propagatorBuilder.addForceModel(new Relativity(gravityField.getMu()));
805 }
806
807
808 if (parser.containsKey(ParameterKey.POLYNOMIAL_ACCELERATION_NAME)) {
809 final String[] names = parser.getStringArray(ParameterKey.POLYNOMIAL_ACCELERATION_NAME);
810 final Vector3D[] directions = parser.getVectorArray(ParameterKey.POLYNOMIAL_ACCELERATION_DIRECTION_X,
811 ParameterKey.POLYNOMIAL_ACCELERATION_DIRECTION_Y,
812 ParameterKey.POLYNOMIAL_ACCELERATION_DIRECTION_Z);
813 final List<String>[] coefficients = parser.getStringsListArray(ParameterKey.POLYNOMIAL_ACCELERATION_COEFFICIENTS, ',');
814 final boolean[] estimated = parser.getBooleanArray(ParameterKey.POLYNOMIAL_ACCELERATION_ESTIMATED);
815
816 for (int i = 0; i < names.length; ++i) {
817
818 final PolynomialParametricAcceleration ppa =
819 new PolynomialParametricAcceleration(directions[i], true, names[i], null,
820 coefficients[i].size() - 1);
821 for (int k = 0; k < coefficients[i].size(); ++k) {
822 final ParameterDriver driver = ppa.getParameterDriver(names[i] + "[" + k + "]");
823 driver.setValue(Double.parseDouble(coefficients[i].get(k)));
824 driver.setSelected(estimated[i]);
825 }
826 propagatorBuilder.addForceModel(ppa);
827 }
828 }
829
830
831 final AttitudeMode mode;
832 if (parser.containsKey(ParameterKey.ATTITUDE_MODE)) {
833 mode = AttitudeMode.valueOf(parser.getString(ParameterKey.ATTITUDE_MODE));
834 } else {
835 mode = AttitudeMode.NADIR_POINTING_WITH_YAW_COMPENSATION;
836 }
837 propagatorBuilder.setAttitudeProvider(mode.getProvider(orbit.getFrame(), body));
838
839 return propagatorBuilder;
840
841 }
842
843
844
845
846
847
848 private NormalizedSphericalHarmonicsProvider createGravityField(final KeyValueFileParser<ParameterKey> parser)
849 throws NoSuchElementException {
850 final int degree = parser.getInt(ParameterKey.CENTRAL_BODY_DEGREE);
851 final int order = FastMath.min(degree, parser.getInt(ParameterKey.CENTRAL_BODY_ORDER));
852 return GravityFieldFactory.getNormalizedProvider(degree, order);
853 }
854
855
856
857
858
859
860 private OneAxisEllipsoid createBody(final KeyValueFileParser<ParameterKey> parser)
861 throws NoSuchElementException {
862
863 final Frame bodyFrame;
864 if (!parser.containsKey(ParameterKey.BODY_FRAME)) {
865 bodyFrame = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
866 } else {
867 bodyFrame = parser.getEarthFrame(ParameterKey.BODY_FRAME);
868 }
869
870 final double equatorialRadius;
871 if (!parser.containsKey(ParameterKey.BODY_EQUATORIAL_RADIUS)) {
872 equatorialRadius = Constants.WGS84_EARTH_EQUATORIAL_RADIUS;
873 } else {
874 equatorialRadius = parser.getDouble(ParameterKey.BODY_EQUATORIAL_RADIUS);
875 }
876
877 final double flattening;
878 if (!parser.containsKey(ParameterKey.BODY_INVERSE_FLATTENING)) {
879 flattening = Constants.WGS84_EARTH_FLATTENING;
880 } else {
881 flattening = 1.0 / parser.getDouble(ParameterKey.BODY_INVERSE_FLATTENING);
882 }
883
884 return new OneAxisEllipsoid(equatorialRadius, flattening, bodyFrame);
885
886 }
887
888
889
890
891
892
893 private Orbit createOrbit(final KeyValueFileParser<ParameterKey> parser,
894 final double mu)
895 throws NoSuchElementException {
896
897 final Frame frame;
898 if (!parser.containsKey(ParameterKey.INERTIAL_FRAME)) {
899 frame = FramesFactory.getEME2000();
900 } else {
901 frame = parser.getInertialFrame(ParameterKey.INERTIAL_FRAME);
902 }
903
904
905 PositionAngle angleType = PositionAngle.MEAN;
906 if (parser.containsKey(ParameterKey.ORBIT_ANGLE_TYPE)) {
907 angleType = PositionAngle.valueOf(parser.getString(ParameterKey.ORBIT_ANGLE_TYPE).toUpperCase());
908 }
909 if (parser.containsKey(ParameterKey.ORBIT_KEPLERIAN_A)) {
910 return new KeplerianOrbit(parser.getDouble(ParameterKey.ORBIT_KEPLERIAN_A),
911 parser.getDouble(ParameterKey.ORBIT_KEPLERIAN_E),
912 parser.getAngle(ParameterKey.ORBIT_KEPLERIAN_I),
913 parser.getAngle(ParameterKey.ORBIT_KEPLERIAN_PA),
914 parser.getAngle(ParameterKey.ORBIT_KEPLERIAN_RAAN),
915 parser.getAngle(ParameterKey.ORBIT_KEPLERIAN_ANOMALY),
916 angleType,
917 frame,
918 parser.getDate(ParameterKey.ORBIT_DATE,
919 TimeScalesFactory.getUTC()),
920 mu);
921 } else if (parser.containsKey(ParameterKey.ORBIT_EQUINOCTIAL_A)) {
922 return new EquinoctialOrbit(parser.getDouble(ParameterKey.ORBIT_EQUINOCTIAL_A),
923 parser.getDouble(ParameterKey.ORBIT_EQUINOCTIAL_EX),
924 parser.getDouble(ParameterKey.ORBIT_EQUINOCTIAL_EY),
925 parser.getDouble(ParameterKey.ORBIT_EQUINOCTIAL_HX),
926 parser.getDouble(ParameterKey.ORBIT_EQUINOCTIAL_HY),
927 parser.getAngle(ParameterKey.ORBIT_EQUINOCTIAL_LAMBDA),
928 angleType,
929 frame,
930 parser.getDate(ParameterKey.ORBIT_DATE,
931 TimeScalesFactory.getUTC()),
932 mu);
933 } else if (parser.containsKey(ParameterKey.ORBIT_CIRCULAR_A)) {
934 return new CircularOrbit(parser.getDouble(ParameterKey.ORBIT_CIRCULAR_A),
935 parser.getDouble(ParameterKey.ORBIT_CIRCULAR_EX),
936 parser.getDouble(ParameterKey.ORBIT_CIRCULAR_EY),
937 parser.getAngle(ParameterKey.ORBIT_CIRCULAR_I),
938 parser.getAngle(ParameterKey.ORBIT_CIRCULAR_RAAN),
939 parser.getAngle(ParameterKey.ORBIT_CIRCULAR_ALPHA),
940 angleType,
941 frame,
942 parser.getDate(ParameterKey.ORBIT_DATE,
943 TimeScalesFactory.getUTC()),
944 mu);
945 } else if (parser.containsKey(ParameterKey.ORBIT_TLE_LINE_1)) {
946 final String line1 = parser.getString(ParameterKey.ORBIT_TLE_LINE_1);
947 final String line2 = parser.getString(ParameterKey.ORBIT_TLE_LINE_2);
948 final TLE tle = new TLE(line1, line2);
949
950 TLEPropagator propagator = TLEPropagator.selectExtrapolator(tle);
951
952
953 AbsoluteDate initDate = tle.getDate();
954 SpacecraftState initialState = propagator.getInitialState();
955
956
957
958 Transform t =FramesFactory.getTEME().getTransformTo(FramesFactory.getEME2000(), initDate.getDate());
959 return new CartesianOrbit( t.transformPVCoordinates(initialState.getPVCoordinates()) ,
960 frame,
961 initDate,
962 mu);
963
964
965 } else {
966 final double[] pos = {parser.getDouble(ParameterKey.ORBIT_CARTESIAN_PX),
967 parser.getDouble(ParameterKey.ORBIT_CARTESIAN_PY),
968 parser.getDouble(ParameterKey.ORBIT_CARTESIAN_PZ)};
969 final double[] vel = {parser.getDouble(ParameterKey.ORBIT_CARTESIAN_VX),
970 parser.getDouble(ParameterKey.ORBIT_CARTESIAN_VY),
971 parser.getDouble(ParameterKey.ORBIT_CARTESIAN_VZ)};
972
973 return new CartesianOrbit(new PVCoordinates(new Vector3D(pos), new Vector3D(vel)),
974 frame,
975 parser.getDate(ParameterKey.ORBIT_DATE,
976 TimeScalesFactory.getUTC()),
977 mu);
978 }
979
980 }
981
982
983
984
985
986 private Bias<Range> createSatRangeBias(final KeyValueFileParser<ParameterKey> parser)
987 {
988
989
990 final double transponderDelayBias;
991 if (!parser.containsKey(ParameterKey.ONBOARD_RANGE_BIAS)) {
992 transponderDelayBias = 0;
993 } else {
994 transponderDelayBias = parser.getDouble(ParameterKey.ONBOARD_RANGE_BIAS);
995 }
996
997 final double transponderDelayBiasMin;
998 if (!parser.containsKey(ParameterKey.ONBOARD_RANGE_BIAS_MIN)) {
999 transponderDelayBiasMin = Double.NEGATIVE_INFINITY;
1000 } else {
1001 transponderDelayBiasMin = parser.getDouble(ParameterKey.ONBOARD_RANGE_BIAS_MIN);
1002 }
1003
1004 final double transponderDelayBiasMax;
1005 if (!parser.containsKey(ParameterKey.ONBOARD_RANGE_BIAS_MAX)) {
1006 transponderDelayBiasMax = Double.NEGATIVE_INFINITY;
1007 } else {
1008 transponderDelayBiasMax = parser.getDouble(ParameterKey.ONBOARD_RANGE_BIAS_MAX);
1009 }
1010
1011
1012 final boolean transponderDelayBiasEstimated;
1013 if (!parser.containsKey(ParameterKey.ONBOARD_RANGE_BIAS_ESTIMATED)) {
1014 transponderDelayBiasEstimated = false;
1015 } else {
1016 transponderDelayBiasEstimated = parser.getBoolean(ParameterKey.ONBOARD_RANGE_BIAS_ESTIMATED);
1017 }
1018
1019 if (FastMath.abs(transponderDelayBias) >= Precision.SAFE_MIN || transponderDelayBiasEstimated) {
1020
1021
1022 final Bias<Range> bias = new Bias<Range>(new String[] {
1023 "transponder delay bias",
1024 },
1025 new double[] {
1026 transponderDelayBias
1027 },
1028 new double[] {
1029 1.0
1030 },
1031 new double[] {
1032 transponderDelayBiasMin
1033 },
1034 new double[] {
1035 transponderDelayBiasMax
1036 });
1037 bias.getParametersDrivers().get(0).setSelected(transponderDelayBiasEstimated);
1038 return bias;
1039 } else {
1040
1041 return null;
1042 }
1043
1044 }
1045
1046
1047
1048
1049
1050 public OnBoardAntennaRangeModifier createSatAntennaRangeModifier(final KeyValueFileParser<ParameterKey> parser) {
1051 final Vector3D offset;
1052 if (!parser.containsKey(ParameterKey.ON_BOARD_ANTENNA_PHASE_CENTER_X)) {
1053 offset = Vector3D.ZERO;
1054 } else {
1055 offset = parser.getVector(ParameterKey.ON_BOARD_ANTENNA_PHASE_CENTER_X,
1056 ParameterKey.ON_BOARD_ANTENNA_PHASE_CENTER_Y,
1057 ParameterKey.ON_BOARD_ANTENNA_PHASE_CENTER_Z);
1058 }
1059 return offset.getNorm() > 0 ? new OnBoardAntennaRangeModifier(offset) : null;
1060 }
1061
1062
1063
1064
1065
1066
1067
1068
1069 private Map<String, StationData> createStationsData(final KeyValueFileParser<ParameterKey> parser,
1070 final IERSConventions conventions,
1071 final OneAxisEllipsoid body)
1072 throws NoSuchElementException {
1073
1074 final Map<String, StationData> stations = new HashMap<String, StationData>();
1075
1076 final String[] stationNames = parser.getStringArray(ParameterKey.GROUND_STATION_NAME);
1077 final double[] stationLatitudes = parser.getAngleArray(ParameterKey.GROUND_STATION_LATITUDE);
1078 final double[] stationLongitudes = parser.getAngleArray(ParameterKey.GROUND_STATION_LONGITUDE);
1079 final double[] stationAltitudes = parser.getDoubleArray(ParameterKey.GROUND_STATION_ALTITUDE);
1080 final boolean[] stationPositionEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_POSITION_ESTIMATED);
1081 final double[] stationClockOffsets = parser.getDoubleArray(ParameterKey.GROUND_STATION_CLOCK_OFFSET);
1082 final double[] stationClockOffsetsMin = parser.getDoubleArray(ParameterKey.GROUND_STATION_CLOCK_OFFSET_MIN);
1083 final double[] stationClockOffsetsMax = parser.getDoubleArray(ParameterKey.GROUND_STATION_CLOCK_OFFSET_MAX);
1084 final boolean[] stationClockOffsetEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_CLOCK_OFFSET_ESTIMATED);
1085 final double[] stationRangeSigma = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_SIGMA);
1086 final double[] stationRangeBias = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_BIAS);
1087 final double[] stationRangeBiasMin = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_BIAS_MIN);
1088 final double[] stationRangeBiasMax = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_BIAS_MAX);
1089 final boolean[] stationRangeBiasEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_RANGE_BIAS_ESTIMATED);
1090 final double[] stationRangeRateSigma = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_SIGMA);
1091 final double[] stationRangeRateBias = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS);
1092 final double[] stationRangeRateBiasMin = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS_MIN);
1093 final double[] stationRangeRateBiasMax = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS_MAX);
1094 final boolean[] stationRangeRateBiasEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS_ESTIMATED);
1095 final double[] stationAzimuthSigma = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_SIGMA);
1096 final double[] stationAzimuthBias = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_BIAS);
1097 final double[] stationAzimuthBiasMin = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_BIAS_MIN);
1098 final double[] stationAzimuthBiasMax = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_BIAS_MAX);
1099 final double[] stationElevationSigma = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_SIGMA);
1100 final double[] stationElevationBias = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_BIAS);
1101 final double[] stationElevationBiasMin = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_BIAS_MIN);
1102 final double[] stationElevationBiasMax = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_BIAS_MAX);
1103 final boolean[] stationAzElBiasesEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_AZ_EL_BIASES_ESTIMATED);
1104 final boolean[] stationElevationRefraction = parser.getBooleanArray(ParameterKey.GROUND_STATION_ELEVATION_REFRACTION_CORRECTION);
1105 final boolean[] stationTroposphericModelEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_TROPOSPHERIC_MODEL_ESTIMATED);
1106 final double[] stationTroposphericZenithDelay = parser.getDoubleArray(ParameterKey.GROUND_STATION_TROPOSPHERIC_ZENITH_DELAY);
1107 final boolean[] stationZenithDelayEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_TROPOSPHERIC_DELAY_ESTIMATED);
1108 final boolean[] stationGlobalMappingFunction = parser.getBooleanArray(ParameterKey.GROUND_STATION_GLOBAL_MAPPING_FUNCTION);
1109 final boolean[] stationNiellMappingFunction = parser.getBooleanArray(ParameterKey.GROUND_STATION_NIELL_MAPPING_FUNCTION);
1110 final boolean[] stationWeatherEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_WEATHER_ESTIMATED);
1111 final boolean[] stationRangeTropospheric = parser.getBooleanArray(ParameterKey.GROUND_STATION_RANGE_TROPOSPHERIC_CORRECTION);
1112
1113
1114 final TidalDisplacement tidalDisplacement;
1115 if (parser.containsKey(ParameterKey.SOLID_TIDES_DISPLACEMENT_CORRECTION) &&
1116 parser.getBoolean(ParameterKey.SOLID_TIDES_DISPLACEMENT_CORRECTION)) {
1117 final boolean removePermanentDeformation =
1118 parser.containsKey(ParameterKey.SOLID_TIDES_DISPLACEMENT_REMOVE_PERMANENT_DEFORMATION) &&
1119 parser.getBoolean(ParameterKey.SOLID_TIDES_DISPLACEMENT_REMOVE_PERMANENT_DEFORMATION);
1120 tidalDisplacement = new TidalDisplacement(Constants.EIGEN5C_EARTH_EQUATORIAL_RADIUS,
1121 Constants.JPL_SSD_SUN_EARTH_PLUS_MOON_MASS_RATIO,
1122 Constants.JPL_SSD_EARTH_MOON_MASS_RATIO,
1123 CelestialBodyFactory.getSun(),
1124 CelestialBodyFactory.getMoon(),
1125 conventions,
1126 removePermanentDeformation);
1127 } else {
1128 tidalDisplacement = null;
1129 }
1130
1131 final OceanLoadingCoefficientsBLQFactory blqFactory;
1132 if (parser.containsKey(ParameterKey.OCEAN_LOADING_CORRECTION) &&
1133 parser.getBoolean(ParameterKey.OCEAN_LOADING_CORRECTION)) {
1134 blqFactory = new OceanLoadingCoefficientsBLQFactory("^.*\\.blq$");
1135 } else {
1136 blqFactory = null;
1137 }
1138
1139 final EOPHistory eopHistory = FramesFactory.findEOP(body.getBodyFrame());
1140 for (int i = 0; i < stationNames.length; ++i) {
1141
1142
1143 final StationDisplacement[] displacements;
1144 final OceanLoading oceanLoading = (blqFactory == null) ?
1145 null :
1146 new OceanLoading(body, blqFactory.getCoefficients(stationNames[i]));
1147 if (tidalDisplacement == null) {
1148 if (oceanLoading == null) {
1149 displacements = new StationDisplacement[0];
1150 } else {
1151 displacements = new StationDisplacement[] {
1152 oceanLoading
1153 };
1154 }
1155 } else {
1156 if (oceanLoading == null) {
1157 displacements = new StationDisplacement[] {
1158 tidalDisplacement
1159 };
1160 } else {
1161 displacements = new StationDisplacement[] {
1162 tidalDisplacement, oceanLoading
1163 };
1164 }
1165 }
1166
1167
1168 final GeodeticPoint position = new GeodeticPoint(stationLatitudes[i],
1169 stationLongitudes[i],
1170 stationAltitudes[i]);
1171 final TopocentricFrame topo = new TopocentricFrame(body, position, stationNames[i]);
1172 final GroundStation station = new GroundStation(topo, eopHistory, displacements);
1173 station.getClockOffsetDriver().setReferenceValue(stationClockOffsets[i]);
1174 station.getClockOffsetDriver().setValue(stationClockOffsets[i]);
1175 station.getClockOffsetDriver().setMinValue(stationClockOffsetsMin[i]);
1176 station.getClockOffsetDriver().setMaxValue(stationClockOffsetsMax[i]);
1177 station.getClockOffsetDriver().setSelected(stationClockOffsetEstimated[i]);
1178 station.getEastOffsetDriver().setSelected(stationPositionEstimated[i]);
1179 station.getNorthOffsetDriver().setSelected(stationPositionEstimated[i]);
1180 station.getZenithOffsetDriver().setSelected(stationPositionEstimated[i]);
1181
1182
1183 final double rangeSigma = stationRangeSigma[i];
1184 final Bias<Range> rangeBias;
1185 if (FastMath.abs(stationRangeBias[i]) >= Precision.SAFE_MIN || stationRangeBiasEstimated[i]) {
1186 rangeBias = new Bias<Range>(new String[] {
1187 stationNames[i] + "/range bias",
1188 },
1189 new double[] {
1190 stationRangeBias[i]
1191 },
1192 new double[] {
1193 rangeSigma
1194 },
1195 new double[] {
1196 stationRangeBiasMin[i]
1197 },
1198 new double[] {
1199 stationRangeBiasMax[i]
1200 });
1201 rangeBias.getParametersDrivers().get(0).setSelected(stationRangeBiasEstimated[i]);
1202 } else {
1203
1204 rangeBias = null;
1205 }
1206
1207
1208 final double rangeRateSigma = stationRangeRateSigma[i];
1209 final Bias<RangeRate> rangeRateBias;
1210 if (FastMath.abs(stationRangeRateBias[i]) >= Precision.SAFE_MIN || stationRangeRateBiasEstimated[i]) {
1211 rangeRateBias = new Bias<RangeRate>(new String[] {
1212 stationNames[i] + "/range rate bias"
1213 },
1214 new double[] {
1215 stationRangeRateBias[i]
1216 },
1217 new double[] {
1218 rangeRateSigma
1219 },
1220 new double[] {
1221 stationRangeRateBiasMin[i]
1222 },
1223 new double[] {
1224 stationRangeRateBiasMax[i]
1225 });
1226 rangeRateBias.getParametersDrivers().get(0).setSelected(stationRangeRateBiasEstimated[i]);
1227 } else {
1228
1229 rangeRateBias = null;
1230 }
1231
1232
1233 final double[] azELSigma = new double[] {
1234 stationAzimuthSigma[i], stationElevationSigma[i]
1235 };
1236 final Bias<AngularAzEl> azELBias;
1237 if (FastMath.abs(stationAzimuthBias[i]) >= Precision.SAFE_MIN ||
1238 FastMath.abs(stationElevationBias[i]) >= Precision.SAFE_MIN ||
1239 stationAzElBiasesEstimated[i]) {
1240 azELBias = new Bias<AngularAzEl>(new String[] {
1241 stationNames[i] + "/az bias",
1242 stationNames[i] + "/el bias"
1243 },
1244 new double[] {
1245 stationAzimuthBias[i],
1246 stationElevationBias[i]
1247 },
1248 azELSigma,
1249 new double[] {
1250 stationAzimuthBiasMin[i],
1251 stationElevationBiasMin[i]
1252 },
1253 new double[] {
1254 stationAzimuthBiasMax[i],
1255 stationElevationBiasMax[i]
1256 });
1257 azELBias.getParametersDrivers().get(0).setSelected(stationAzElBiasesEstimated[i]);
1258 azELBias.getParametersDrivers().get(1).setSelected(stationAzElBiasesEstimated[i]);
1259 } else {
1260
1261 azELBias = null;
1262 }
1263
1264
1265 final AngularRadioRefractionModifier refractionCorrection;
1266 if (stationElevationRefraction[i]) {
1267 final double altitude = station.getBaseFrame().getPoint().getAltitude();
1268 final AtmosphericRefractionModel refractionModel = new EarthITU453AtmosphereRefraction(altitude);
1269 refractionCorrection = new AngularRadioRefractionModifier(refractionModel);
1270 } else {
1271 refractionCorrection = null;
1272 }
1273
1274
1275
1276 final RangeTroposphericDelayModifier rangeTroposphericCorrection;
1277 if (stationRangeTropospheric[i]) {
1278
1279 MappingFunction mappingModel = null;
1280 if (stationGlobalMappingFunction[i]) {
1281 mappingModel = new GlobalMappingFunctionModel(stationLatitudes[i],
1282 stationLongitudes[i]);
1283 } else if (stationNiellMappingFunction[i]) {
1284 mappingModel = new NiellMappingFunctionModel(stationLatitudes[i]);
1285 }
1286
1287 DiscreteTroposphericModel troposphericModel;
1288 if (stationTroposphericModelEstimated[i] && mappingModel != null) {
1289
1290 if(stationWeatherEstimated[i]) {
1291 final GlobalPressureTemperatureModel weather = new GlobalPressureTemperatureModel(stationLatitudes[i],
1292 stationLongitudes[i],
1293 body.getBodyFrame());
1294 weather.weatherParameters(stationAltitudes[i], parser.getDate(ParameterKey.ORBIT_DATE,
1295 TimeScalesFactory.getUTC()));
1296 final double temperature = weather.getTemperature();
1297 final double pressure = weather.getPressure();
1298 troposphericModel = new EstimatedTroposphericModel(temperature, pressure, mappingModel,
1299 stationTroposphericZenithDelay[i]);
1300 } else {
1301 troposphericModel = new EstimatedTroposphericModel(mappingModel, stationTroposphericZenithDelay[i]);
1302 }
1303
1304 ParameterDriver totalDelay = troposphericModel.getParametersDrivers().get(0);
1305 totalDelay.setSelected(stationZenithDelayEstimated[i]);
1306 totalDelay.setName(stationNames[i].substring(0, 5) + EstimatedTroposphericModel.TOTAL_ZENITH_DELAY);
1307 } else {
1308 troposphericModel = SaastamoinenModel.getStandardModel();
1309 }
1310
1311 rangeTroposphericCorrection = new RangeTroposphericDelayModifier(troposphericModel);
1312 } else {
1313 rangeTroposphericCorrection = null;
1314 }
1315
1316 stations.put(stationNames[i], new StationData(station,
1317 rangeSigma, rangeBias,
1318 rangeRateSigma, rangeRateBias,
1319 azELSigma, azELBias,
1320 refractionCorrection, rangeTroposphericCorrection));
1321 }
1322 return stations;
1323
1324 }
1325
1326
1327
1328
1329
1330
1331 private Weights createWeights(final KeyValueFileParser<ParameterKey> parser)
1332 throws NoSuchElementException {
1333 return new Weights(parser.getDouble(ParameterKey.RANGE_MEASUREMENTS_BASE_WEIGHT),
1334 parser.getDouble(ParameterKey.RANGE_RATE_MEASUREMENTS_BASE_WEIGHT),
1335 new double[] {
1336 parser.getDouble(ParameterKey.AZIMUTH_MEASUREMENTS_BASE_WEIGHT),
1337 parser.getDouble(ParameterKey.ELEVATION_MEASUREMENTS_BASE_WEIGHT)
1338 },
1339 parser.getDouble(ParameterKey.PV_MEASUREMENTS_BASE_WEIGHT));
1340 }
1341
1342
1343
1344
1345
1346 private OutlierFilter<Range> createRangeOutliersManager(final KeyValueFileParser<ParameterKey> parser) {
1347 if (parser.containsKey(ParameterKey.RANGE_OUTLIER_REJECTION_MULTIPLIER) !=
1348 parser.containsKey(ParameterKey.RANGE_OUTLIER_REJECTION_STARTING_ITERATION)) {
1349 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
1350 ParameterKey.RANGE_OUTLIER_REJECTION_MULTIPLIER.toString().toLowerCase().replace('_', '.') +
1351 " and " +
1352 ParameterKey.RANGE_OUTLIER_REJECTION_STARTING_ITERATION.toString().toLowerCase().replace('_', '.') +
1353 " must be both present or both absent");
1354 }
1355 return new OutlierFilter<Range>(parser.getInt(ParameterKey.RANGE_OUTLIER_REJECTION_STARTING_ITERATION),
1356 parser.getInt(ParameterKey.RANGE_OUTLIER_REJECTION_MULTIPLIER));
1357 }
1358
1359
1360
1361
1362
1363 private OutlierFilter<RangeRate> createRangeRateOutliersManager(final KeyValueFileParser<ParameterKey> parser) {
1364 if (parser.containsKey(ParameterKey.RANGE_RATE_OUTLIER_REJECTION_MULTIPLIER) !=
1365 parser.containsKey(ParameterKey.RANGE_RATE_OUTLIER_REJECTION_STARTING_ITERATION)) {
1366 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
1367 ParameterKey.RANGE_RATE_OUTLIER_REJECTION_MULTIPLIER.toString().toLowerCase().replace('_', '.') +
1368 " and " +
1369 ParameterKey.RANGE_RATE_OUTLIER_REJECTION_STARTING_ITERATION.toString().toLowerCase().replace('_', '.') +
1370 " must be both present or both absent");
1371 }
1372 return new OutlierFilter<RangeRate>(parser.getInt(ParameterKey.RANGE_RATE_OUTLIER_REJECTION_STARTING_ITERATION),
1373 parser.getInt(ParameterKey.RANGE_RATE_OUTLIER_REJECTION_MULTIPLIER));
1374 }
1375
1376
1377
1378
1379
1380 private OutlierFilter<AngularAzEl> createAzElOutliersManager(final KeyValueFileParser<ParameterKey> parser) {
1381 if (parser.containsKey(ParameterKey.AZ_EL_OUTLIER_REJECTION_MULTIPLIER) !=
1382 parser.containsKey(ParameterKey.AZ_EL_OUTLIER_REJECTION_STARTING_ITERATION)) {
1383 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
1384 ParameterKey.AZ_EL_OUTLIER_REJECTION_MULTIPLIER.toString().toLowerCase().replace('_', '.') +
1385 " and " +
1386 ParameterKey.AZ_EL_OUTLIER_REJECTION_STARTING_ITERATION.toString().toLowerCase().replace('_', '.') +
1387 " must be both present or both absent");
1388 }
1389 return new OutlierFilter<AngularAzEl>(parser.getInt(ParameterKey.AZ_EL_OUTLIER_REJECTION_STARTING_ITERATION),
1390 parser.getInt(ParameterKey.AZ_EL_OUTLIER_REJECTION_MULTIPLIER));
1391 }
1392
1393
1394
1395
1396
1397 private OutlierFilter<PV> createPVOutliersManager(final KeyValueFileParser<ParameterKey> parser) {
1398 if (parser.containsKey(ParameterKey.PV_OUTLIER_REJECTION_MULTIPLIER) !=
1399 parser.containsKey(ParameterKey.PV_OUTLIER_REJECTION_STARTING_ITERATION)) {
1400 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
1401 ParameterKey.PV_OUTLIER_REJECTION_MULTIPLIER.toString().toLowerCase().replace('_', '.') +
1402 " and " +
1403 ParameterKey.PV_OUTLIER_REJECTION_STARTING_ITERATION.toString().toLowerCase().replace('_', '.') +
1404 " must be both present or both absent");
1405 }
1406 return new OutlierFilter<PV>(parser.getInt(ParameterKey.PV_OUTLIER_REJECTION_STARTING_ITERATION),
1407 parser.getInt(ParameterKey.PV_OUTLIER_REJECTION_MULTIPLIER));
1408 }
1409
1410
1411
1412
1413
1414
1415 private PVData createPVData(final KeyValueFileParser<ParameterKey> parser)
1416 throws NoSuchElementException {
1417 return new PVData(parser.getDouble(ParameterKey.PV_MEASUREMENTS_POSITION_SIGMA),
1418 parser.getDouble(ParameterKey.PV_MEASUREMENTS_VELOCITY_SIGMA));
1419 }
1420
1421
1422
1423
1424
1425
1426 private ObservableSatellite createObservableSatellite(final KeyValueFileParser<ParameterKey> parser)
1427 throws NoSuchElementException {
1428 ObservableSatellite obsSat = new ObservableSatellite(0);
1429 ParameterDriver clockOffsetDriver = obsSat.getClockOffsetDriver();
1430 if (parser.containsKey(ParameterKey.ON_BOARD_CLOCK_OFFSET)) {
1431 clockOffsetDriver.setReferenceValue(parser.getDouble(ParameterKey.ON_BOARD_CLOCK_OFFSET));
1432 clockOffsetDriver.setValue(parser.getDouble(ParameterKey.ON_BOARD_CLOCK_OFFSET));
1433 }
1434 if (parser.containsKey(ParameterKey.ON_BOARD_CLOCK_OFFSET_MIN)) {
1435 clockOffsetDriver.setMinValue(parser.getDouble(ParameterKey.ON_BOARD_CLOCK_OFFSET_MIN));
1436 }
1437 if (parser.containsKey(ParameterKey.ON_BOARD_CLOCK_OFFSET_MAX)) {
1438 clockOffsetDriver.setMaxValue(parser.getDouble(ParameterKey.ON_BOARD_CLOCK_OFFSET_MAX));
1439 }
1440 if (parser.containsKey(ParameterKey.ON_BOARD_CLOCK_OFFSET_ESTIMATED)) {
1441 clockOffsetDriver.setSelected(parser.getBoolean(ParameterKey.ON_BOARD_CLOCK_OFFSET_ESTIMATED));
1442 }
1443 return obsSat;
1444 }
1445
1446
1447
1448
1449
1450
1451
1452 private BatchLSEstimator createEstimator(final KeyValueFileParser<ParameterKey> parser,
1453 final NumericalPropagatorBuilder propagatorBuilder)
1454 throws NoSuchElementException {
1455 final boolean optimizerIsLevenbergMarquardt;
1456 if (! parser.containsKey(ParameterKey.ESTIMATOR_OPTIMIZATION_ENGINE)) {
1457 optimizerIsLevenbergMarquardt = true;
1458 } else {
1459 final String engine = parser.getString(ParameterKey.ESTIMATOR_OPTIMIZATION_ENGINE);
1460 optimizerIsLevenbergMarquardt = engine.toLowerCase().contains("levenberg");
1461 }
1462 final LeastSquaresOptimizer optimizer;
1463
1464 if (optimizerIsLevenbergMarquardt) {
1465
1466 final double initialStepBoundFactor;
1467 if (! parser.containsKey(ParameterKey.ESTIMATOR_LEVENBERG_MARQUARDT_INITIAL_STEP_BOUND_FACTOR)) {
1468 initialStepBoundFactor = 100.0;
1469 } else {
1470 initialStepBoundFactor = parser.getDouble(ParameterKey.ESTIMATOR_LEVENBERG_MARQUARDT_INITIAL_STEP_BOUND_FACTOR);
1471 }
1472
1473 optimizer = new LevenbergMarquardtOptimizer().withInitialStepBoundFactor(initialStepBoundFactor);
1474 } else {
1475
1476 optimizer = new GaussNewtonOptimizer(new QRDecomposer(1e-11), false);
1477 }
1478
1479 final double convergenceThreshold;
1480 if (! parser.containsKey(ParameterKey.ESTIMATOR_NORMALIZED_PARAMETERS_CONVERGENCE_THRESHOLD)) {
1481 convergenceThreshold = 1.0e-3;
1482 } else {
1483 convergenceThreshold = parser.getDouble(ParameterKey.ESTIMATOR_NORMALIZED_PARAMETERS_CONVERGENCE_THRESHOLD);
1484 }
1485 final int maxIterations;
1486 if (! parser.containsKey(ParameterKey.ESTIMATOR_MAX_ITERATIONS)) {
1487 maxIterations = 10;
1488 } else {
1489 maxIterations = parser.getInt(ParameterKey.ESTIMATOR_MAX_ITERATIONS);
1490 }
1491 final int maxEvaluations;
1492 if (! parser.containsKey(ParameterKey.ESTIMATOR_MAX_EVALUATIONS)) {
1493 maxEvaluations = 20;
1494 } else {
1495 maxEvaluations = parser.getInt(ParameterKey.ESTIMATOR_MAX_EVALUATIONS);
1496 }
1497
1498 final BatchLSEstimator estimator = new BatchLSEstimator(optimizer, propagatorBuilder);
1499 estimator.setParametersConvergenceThreshold(convergenceThreshold);
1500 estimator.setMaxIterations(maxIterations);
1501 estimator.setMaxEvaluations(maxEvaluations);
1502
1503 return estimator;
1504
1505 }
1506
1507
1508
1509
1510
1511
1512
1513
1514
1515
1516
1517
1518
1519 private List<ObservedMeasurement<?>> readRinex(final File file, final String satId,
1520 final Map<String, StationData> stations,
1521 final ObservableSatellite satellite,
1522 final Bias<Range> satRangeBias,
1523 final OnBoardAntennaRangeModifier satAntennaRangeModifier,
1524 final Weights weights,
1525 final OutlierFilter<Range> rangeOutliersManager,
1526 final OutlierFilter<RangeRate> rangeRateOutliersManager)
1527 throws UnsupportedEncodingException, IOException, OrekitException {
1528 final List<ObservedMeasurement<?>> measurements = new ArrayList<ObservedMeasurement<?>>();
1529 final SatelliteSystem system = SatelliteSystem.parseSatelliteSystem(satId);
1530 final int prnNumber;
1531 switch (system) {
1532 case GPS:
1533 case GLONASS:
1534 case GALILEO:
1535 prnNumber = Integer.parseInt(satId.substring(1));
1536 break;
1537 case SBAS:
1538 prnNumber = Integer.parseInt(satId.substring(1)) + 100;
1539 break;
1540 default:
1541 prnNumber = -1;
1542 }
1543 final Iono iono = new Iono(false);
1544 final RinexLoader loader = new RinexLoader(new FileInputStream(file), file.getAbsolutePath());
1545 for (final ObservationDataSet observationDataSet : loader.getObservationDataSets()) {
1546 if (observationDataSet.getSatelliteSystem() == system &&
1547 observationDataSet.getPrnNumber() == prnNumber) {
1548 for (final ObservationData od : observationDataSet.getObservationData()) {
1549 if (!Double.isNaN(od.getValue())) {
1550 if (od.getObservationType().getMeasurementType() == MeasurementType.PSEUDO_RANGE) {
1551
1552 final String stationName = observationDataSet.getHeader().getMarkerName() + "/" + od.getObservationType();
1553 StationData stationData = stations.get(stationName);
1554 if (stationData == null) {
1555 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
1556 stationName + " not configured");
1557 }
1558 Range range = new Range(stationData.station, false, observationDataSet.getDate(),
1559 od.getValue(), stationData.rangeSigma,
1560 weights.rangeBaseWeight, satellite);
1561 range.addModifier(iono.getRangeModifier(od.getObservationType().getFrequency(system),
1562 observationDataSet.getDate()));
1563 if (satAntennaRangeModifier != null) {
1564 range.addModifier(satAntennaRangeModifier);
1565 }
1566 if (stationData.rangeBias != null) {
1567 range.addModifier(stationData.rangeBias);
1568 }
1569 if (satRangeBias != null) {
1570 range.addModifier(satRangeBias);
1571 }
1572 if (stationData.rangeTroposphericCorrection != null) {
1573 range.addModifier(stationData.rangeTroposphericCorrection);
1574 }
1575 addIfNonZeroWeight(range, measurements);
1576
1577 } else if (od.getObservationType().getMeasurementType() == MeasurementType.DOPPLER) {
1578
1579 final String stationName = observationDataSet.getHeader().getMarkerName() + "/" + od.getObservationType();
1580 StationData stationData = stations.get(stationName);
1581 if (stationData == null) {
1582 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
1583 stationName + " not configured");
1584 }
1585 RangeRate rangeRate = new RangeRate(stationData.station, observationDataSet.getDate(),
1586 od.getValue(), stationData.rangeRateSigma,
1587 weights.rangeRateBaseWeight, false, satellite);
1588 rangeRate.addModifier(iono.getRangeRateModifier(od.getObservationType().getFrequency(system),
1589 observationDataSet.getDate()));
1590 if (stationData.rangeRateBias != null) {
1591 rangeRate.addModifier(stationData.rangeRateBias);
1592 }
1593 addIfNonZeroWeight(rangeRate, measurements);
1594 }
1595 }
1596 }
1597 }
1598 }
1599
1600 return measurements;
1601
1602 }
1603
1604
1605
1606
1607
1608
1609
1610
1611
1612
1613
1614
1615
1616
1617
1618 private List<ObservedMeasurement<?>> readMeasurements(final File file,
1619 final Map<String, StationData> stations,
1620 final PVData pvData,
1621 final ObservableSatellite satellite,
1622 final Bias<Range> satRangeBias,
1623 final OnBoardAntennaRangeModifier satAntennaRangeModifier,
1624 final Weights weights,
1625 final OutlierFilter<Range> rangeOutliersManager,
1626 final OutlierFilter<RangeRate> rangeRateOutliersManager,
1627 final OutlierFilter<AngularAzEl> azElOutliersManager,
1628 final OutlierFilter<PV> pvOutliersManager)
1629 throws UnsupportedEncodingException, IOException, OrekitException {
1630
1631 final List<ObservedMeasurement<?>> measurements = new ArrayList<ObservedMeasurement<?>>();
1632 BufferedReader br = null;
1633 try {
1634 br = new BufferedReader(new InputStreamReader(new FileInputStream(file), "UTF-8"));
1635 int lineNumber = 0;
1636 for (String line = br.readLine(); line != null; line = br.readLine()) {
1637 ++lineNumber;
1638 line = line.trim();
1639 if (line.length() > 0 && !line.startsWith("#")) {
1640 String[] fields = line.split("\\s+");
1641 if (fields.length < 2) {
1642 throw new OrekitException(OrekitMessages.UNABLE_TO_PARSE_LINE_IN_FILE,
1643 lineNumber, file.getName(), line);
1644 }
1645 switch (fields[1]) {
1646 case "RANGE" :
1647 final Range range = new RangeParser().parseFields(fields, stations, pvData, satellite,
1648 satRangeBias, weights,
1649 line, lineNumber, file.getName());
1650 if (satAntennaRangeModifier != null) {
1651 range.addModifier(satAntennaRangeModifier);
1652 }
1653 if (rangeOutliersManager != null) {
1654 range.addModifier(rangeOutliersManager);
1655 }
1656 addIfNonZeroWeight(range, measurements);
1657 break;
1658 case "RANGE_RATE" :
1659 final RangeRate rangeRate = new RangeRateParser().parseFields(fields, stations, pvData, satellite,
1660 satRangeBias, weights,
1661 line, lineNumber, file.getName());
1662 if (rangeOutliersManager != null) {
1663 rangeRate.addModifier(rangeRateOutliersManager);
1664 }
1665 addIfNonZeroWeight(rangeRate, measurements);
1666 break;
1667 case "AZ_EL" :
1668 final AngularAzEl angular = new AzElParser().parseFields(fields, stations, pvData, satellite,
1669 satRangeBias, weights,
1670 line, lineNumber, file.getName());
1671 if (azElOutliersManager != null) {
1672 angular.addModifier(azElOutliersManager);
1673 }
1674 addIfNonZeroWeight(angular, measurements);
1675 break;
1676 case "PV" :
1677 final PV pv = new PVParser().parseFields(fields, stations, pvData, satellite,
1678 satRangeBias, weights,
1679 line, lineNumber, file.getName());
1680 if (pvOutliersManager != null) {
1681 pv.addModifier(pvOutliersManager);
1682 }
1683 addIfNonZeroWeight(pv, measurements);
1684 break;
1685 default :
1686 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
1687 "unknown measurement type " + fields[1] +
1688 " at line " + lineNumber +
1689 " in file " + file.getName());
1690 }
1691 }
1692 }
1693 } finally {
1694 if (br != null) {
1695 br.close();
1696 }
1697 }
1698
1699 if (measurements.isEmpty()) {
1700 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
1701 "not measurements read from file " + file.getAbsolutePath());
1702 }
1703
1704 return measurements;
1705
1706 }
1707
1708
1709
1710
1711
1712 private void addIfNonZeroWeight(final ObservedMeasurement<?> measurement, final List<ObservedMeasurement<?>> measurements) {
1713 double sum = 0;
1714 for (double w : measurement.getBaseWeight()) {
1715 sum += FastMath.abs(w);
1716 }
1717 if (sum > Precision.SAFE_MIN) {
1718
1719 measurements.add(measurement);
1720 }
1721 }
1722
1723
1724 private static class StationData {
1725
1726
1727 private final GroundStation station;
1728
1729
1730 private final double rangeSigma;
1731
1732
1733 private final Bias<Range> rangeBias;
1734
1735
1736 private final double rangeRateSigma;
1737
1738
1739 private final Bias<RangeRate> rangeRateBias;
1740
1741
1742 private final double[] azElSigma;
1743
1744
1745 private final Bias<AngularAzEl> azELBias;
1746
1747
1748 private final AngularRadioRefractionModifier refractionCorrection;
1749
1750
1751 private final RangeTroposphericDelayModifier rangeTroposphericCorrection;
1752
1753
1754
1755
1756
1757
1758
1759
1760
1761
1762
1763
1764 public StationData(final GroundStation station,
1765 final double rangeSigma, final Bias<Range> rangeBias,
1766 final double rangeRateSigma, final Bias<RangeRate> rangeRateBias,
1767 final double[] azElSigma, final Bias<AngularAzEl> azELBias,
1768 final AngularRadioRefractionModifier refractionCorrection,
1769 final RangeTroposphericDelayModifier rangeTroposphericCorrection) {
1770 this.station = station;
1771 this.rangeSigma = rangeSigma;
1772 this.rangeBias = rangeBias;
1773 this.rangeRateSigma = rangeRateSigma;
1774 this.rangeRateBias = rangeRateBias;
1775 this.azElSigma = azElSigma.clone();
1776 this.azELBias = azELBias;
1777 this.refractionCorrection = refractionCorrection;
1778 this.rangeTroposphericCorrection = rangeTroposphericCorrection;
1779 }
1780
1781 }
1782
1783
1784 private static class Weights {
1785
1786
1787 private final double rangeBaseWeight;
1788
1789
1790 private final double rangeRateBaseWeight;
1791
1792
1793 private final double[] azElBaseWeight;
1794
1795
1796 private final double pvBaseWeight;
1797
1798
1799
1800
1801
1802
1803
1804 public Weights(final double rangeBaseWeight,
1805 final double rangeRateBaseWeight,
1806 final double[] azElBaseWeight,
1807 final double pvBaseWeight) {
1808 this.rangeBaseWeight = rangeBaseWeight;
1809 this.rangeRateBaseWeight = rangeRateBaseWeight;
1810 this.azElBaseWeight = azElBaseWeight.clone();
1811 this.pvBaseWeight = pvBaseWeight;
1812 }
1813
1814 }
1815
1816
1817 private static class PVData {
1818
1819
1820 private final double positionSigma;
1821
1822
1823 private final double velocitySigma;
1824
1825
1826
1827
1828
1829 public PVData(final double positionSigma, final double velocitySigma) {
1830 this.positionSigma = positionSigma;
1831 this.velocitySigma = velocitySigma;
1832 }
1833
1834 }
1835
1836
1837 private static abstract class MeasurementsParser<T extends ObservedMeasurement<T>> {
1838
1839
1840
1841
1842
1843
1844
1845
1846
1847
1848
1849
1850
1851 public abstract T parseFields(String[] fields,
1852 Map<String, StationData> stations,
1853 PVData pvData, ObservableSatellite satellite,
1854 Bias<Range> satRangeBias, Weights weight,
1855 String line, int lineNumber, String fileName);
1856
1857
1858
1859
1860
1861
1862
1863
1864 protected void checkFields(final int expected, final String[] fields,
1865 final String line, final int lineNumber, final String fileName) {
1866 if (fields.length != expected) {
1867 throw new OrekitException(OrekitMessages.UNABLE_TO_PARSE_LINE_IN_FILE,
1868 lineNumber, fileName, line);
1869 }
1870 }
1871
1872
1873
1874
1875
1876
1877
1878
1879 protected AbsoluteDate getDate(final String date,
1880 final String line, final int lineNumber, final String fileName) {
1881 try {
1882 return new AbsoluteDate(date, TimeScalesFactory.getUTC());
1883 } catch (OrekitException oe) {
1884 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
1885 "wrong date " + date +
1886 " at line " + lineNumber +
1887 " in file " + fileName +
1888 "\n" + line);
1889 }
1890 }
1891
1892
1893
1894
1895
1896
1897
1898
1899
1900 protected StationData getStationData(final String stationName,
1901 final Map<String, StationData> stations,
1902 final String line, final int lineNumber, final String fileName) {
1903 final StationData stationData = stations.get(stationName);
1904 if (stationData == null) {
1905 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
1906 "unknown station " + stationName +
1907 " at line " + lineNumber +
1908 " in file " + fileName +
1909 "\n" + line);
1910 }
1911 return stationData;
1912 }
1913 }
1914
1915
1916 private static class RangeParser extends MeasurementsParser<Range> {
1917
1918 @Override
1919 public Range parseFields(final String[] fields,
1920 final Map<String, StationData> stations,
1921 final PVData pvData,
1922 final ObservableSatellite satellite,
1923 final Bias<Range> satRangeBias,
1924 final Weights weights,
1925 final String line,
1926 final int lineNumber,
1927 final String fileName) {
1928 checkFields(4, fields, line, lineNumber, fileName);
1929 final StationData stationData = getStationData(fields[2], stations, line, lineNumber, fileName);
1930 final Range range = new Range(stationData.station, true,
1931 getDate(fields[0], line, lineNumber, fileName),
1932 Double.parseDouble(fields[3]) * 1000.0,
1933 stationData.rangeSigma,
1934 weights.rangeBaseWeight,
1935 satellite);
1936 if (stationData.rangeBias != null) {
1937 range.addModifier(stationData.rangeBias);
1938 }
1939 if (satRangeBias != null) {
1940 range.addModifier(satRangeBias);
1941 }
1942 if (stationData.rangeTroposphericCorrection != null) {
1943 range.addModifier(stationData.rangeTroposphericCorrection);
1944 }
1945 return range;
1946 }
1947 }
1948
1949
1950 private static class RangeRateParser extends MeasurementsParser<RangeRate> {
1951
1952 @Override
1953 public RangeRate parseFields(final String[] fields,
1954 final Map<String, StationData> stations,
1955 final PVData pvData,
1956 final ObservableSatellite satellite,
1957 final Bias<Range> satRangeBias,
1958 final Weights weights,
1959 final String line,
1960 final int lineNumber,
1961 final String fileName) {
1962 checkFields(4, fields, line, lineNumber, fileName);
1963 final StationData stationData = getStationData(fields[2], stations, line, lineNumber, fileName);
1964 final RangeRate rangeRate = new RangeRate(stationData.station,
1965 getDate(fields[0], line, lineNumber, fileName),
1966 Double.parseDouble(fields[3]) * 1000.0,
1967 stationData.rangeRateSigma,
1968 weights.rangeRateBaseWeight,
1969 true, satellite);
1970 if (stationData.rangeRateBias != null) {
1971 rangeRate.addModifier(stationData.rangeRateBias);
1972 }
1973 return rangeRate;
1974 }
1975 };
1976
1977
1978 private static class AzElParser extends MeasurementsParser<AngularAzEl> {
1979
1980 @Override
1981 public AngularAzEl parseFields(final String[] fields,
1982 final Map<String, StationData> stations,
1983 final PVData pvData,
1984 final ObservableSatellite satellite,
1985 final Bias<Range> satRangeBias,
1986 final Weights weights,
1987 final String line,
1988 final int lineNumber,
1989 final String fileName) {
1990 checkFields(5, fields, line, lineNumber, fileName);
1991 final StationData stationData = getStationData(fields[2], stations, line, lineNumber, fileName);
1992 final AngularAzEl azEl = new AngularAzEl(stationData.station,
1993 getDate(fields[0], line, lineNumber, fileName),
1994 new double[] {
1995 FastMath.toRadians(Double.parseDouble(fields[3])),
1996 FastMath.toRadians(Double.parseDouble(fields[4]))
1997 },
1998 stationData.azElSigma,
1999 weights.azElBaseWeight,
2000 satellite);
2001 if (stationData.refractionCorrection != null) {
2002 azEl.addModifier(stationData.refractionCorrection);
2003 }
2004 if (stationData.azELBias != null) {
2005 azEl.addModifier(stationData.azELBias);
2006 }
2007 return azEl;
2008 }
2009 };
2010
2011
2012 private static class PVParser extends MeasurementsParser<PV> {
2013
2014 @Override
2015 public PV parseFields(final String[] fields,
2016 final Map<String, StationData> stations,
2017 final PVData pvData,
2018 final ObservableSatellite satellite,
2019 final Bias<Range> satRangeBias,
2020 final Weights weights,
2021 final String line,
2022 final int lineNumber,
2023 final String fileName) {
2024
2025
2026
2027 checkFields(9, fields, line, lineNumber, fileName);
2028 return new org.orekit.estimation.measurements.PV(getDate(fields[0], line, lineNumber, fileName),
2029 new Vector3D(Double.parseDouble(fields[3]) * 1000.0,
2030 Double.parseDouble(fields[4]) * 1000.0,
2031 Double.parseDouble(fields[5]) * 1000.0),
2032 new Vector3D(Double.parseDouble(fields[6]) * 1000.0,
2033 Double.parseDouble(fields[7]) * 1000.0,
2034 Double.parseDouble(fields[8]) * 1000.0),
2035 pvData.positionSigma,
2036 pvData.velocitySigma,
2037 weights.pvBaseWeight,
2038 satellite);
2039 }
2040 };
2041
2042
2043
2044
2045 private static abstract class MeasurementLog<T extends ObservedMeasurement<T>> {
2046
2047
2048 private final SortedSet<EstimatedMeasurement<T>> evaluations;
2049
2050
2051 private final String name;
2052
2053
2054
2055
2056
2057
2058 MeasurementLog(final String name) throws IOException {
2059 this.evaluations = new TreeSet<EstimatedMeasurement<T>>(Comparator.naturalOrder());
2060 this.name = name;
2061 }
2062
2063
2064
2065
2066 public String getName() {
2067 return name;
2068 }
2069
2070
2071
2072
2073 abstract double residual(final EstimatedMeasurement<T> evaluation);
2074
2075
2076
2077
2078 void add(final EstimatedMeasurement<T> evaluation) {
2079 evaluations.add(evaluation);
2080 }
2081
2082
2083
2084 public StreamingStatistics createStatisticsSummary() {
2085 if (!evaluations.isEmpty()) {
2086
2087 final StreamingStatistics stats = new StreamingStatistics();
2088 for (final EstimatedMeasurement<T> evaluation : evaluations) {
2089 stats.addValue(residual(evaluation));
2090 }
2091 return stats;
2092
2093 }
2094 return null;
2095 }
2096 }
2097
2098
2099 class RangeLog extends MeasurementLog<Range> {
2100
2101
2102
2103
2104 RangeLog() throws IOException {
2105 super("range");
2106 }
2107
2108
2109 @Override
2110 double residual(final EstimatedMeasurement<Range> evaluation) {
2111 return evaluation.getEstimatedValue()[0] - evaluation.getObservedMeasurement().getObservedValue()[0];
2112 }
2113
2114 }
2115
2116
2117 class RangeRateLog extends MeasurementLog<RangeRate> {
2118
2119
2120
2121
2122 RangeRateLog() throws IOException {
2123 super("range-rate");
2124 }
2125
2126
2127 @Override
2128 double residual(final EstimatedMeasurement<RangeRate> evaluation) {
2129 return evaluation.getEstimatedValue()[0] - evaluation.getObservedMeasurement().getObservedValue()[0];
2130 }
2131
2132 }
2133
2134
2135 class AzimuthLog extends MeasurementLog<AngularAzEl> {
2136
2137
2138
2139
2140 AzimuthLog() throws IOException {
2141 super("azimuth");
2142 }
2143
2144
2145 @Override
2146 double residual(final EstimatedMeasurement<AngularAzEl> evaluation) {
2147 return FastMath.toDegrees(evaluation.getEstimatedValue()[0] - evaluation.getObservedMeasurement().getObservedValue()[0]);
2148 }
2149
2150 }
2151
2152
2153 class ElevationLog extends MeasurementLog<AngularAzEl> {
2154
2155
2156
2157
2158
2159
2160 ElevationLog() throws IOException {
2161 super("elevation");
2162 }
2163
2164
2165 @Override
2166 double residual(final EstimatedMeasurement<AngularAzEl> evaluation) {
2167 return FastMath.toDegrees(evaluation.getEstimatedValue()[1] - evaluation.getObservedMeasurement().getObservedValue()[1]);
2168 }
2169
2170 }
2171
2172
2173 class PositionLog extends MeasurementLog<PV> {
2174
2175
2176
2177
2178
2179
2180 PositionLog() throws IOException {
2181 super("position");
2182 }
2183
2184
2185 @Override
2186 double residual(final EstimatedMeasurement<PV> evaluation) {
2187 final double[] theoretical = evaluation.getEstimatedValue();
2188 final double[] observed = evaluation.getObservedMeasurement().getObservedValue();
2189 return Vector3D.distance(new Vector3D(theoretical[0], theoretical[1], theoretical[2]),
2190 new Vector3D(observed[0], observed[1], observed[2]));
2191 }
2192
2193 }
2194
2195
2196 class VelocityLog extends MeasurementLog<PV> {
2197
2198
2199
2200
2201
2202
2203 VelocityLog() throws IOException {
2204 super("velocity");
2205 }
2206
2207
2208 @Override
2209 double residual(final EstimatedMeasurement<PV> evaluation) {
2210 final double[] theoretical = evaluation.getEstimatedValue();
2211 final double[] observed = evaluation.getObservedMeasurement().getObservedValue();
2212 return Vector3D.distance(new Vector3D(theoretical[3], theoretical[4], theoretical[5]),
2213 new Vector3D(observed[3], observed[4], observed[5]));
2214 }
2215
2216 }
2217
2218 private static class EvaluationCounter<T extends ObservedMeasurement<T>> {
2219
2220
2221 private int total;
2222
2223
2224 private int active;
2225
2226
2227
2228
2229 public void add(EstimatedMeasurement<T> evaluation) {
2230 ++total;
2231 if (evaluation.getStatus() == EstimatedMeasurement.Status.PROCESSED) {
2232 ++active;
2233 }
2234 }
2235
2236
2237
2238
2239 public String format(final int size) {
2240 StringBuilder builder = new StringBuilder();
2241 builder.append(active);
2242 builder.append('/');
2243 builder.append(total);
2244 while (builder.length() < size) {
2245 if (builder.length() % 2 == 0) {
2246 builder.insert(0, ' ');
2247 } else {
2248 builder.append(' ');
2249 }
2250 }
2251 return builder.toString();
2252 }
2253
2254 }
2255
2256
2257 private static class Iono {
2258
2259
2260 private final boolean twoWay;
2261
2262
2263 private final Map<Frequency, Map<DateComponents, RangeIonosphericDelayModifier>> rangeModifiers;
2264
2265
2266 private final Map<Frequency, Map<DateComponents, RangeRateIonosphericDelayModifier>> rangeRateModifiers;
2267
2268
2269
2270
2271 Iono(final boolean twoWay) {
2272 this.twoWay = twoWay;
2273 this.rangeModifiers = new HashMap<>();
2274 this.rangeRateModifiers = new HashMap<>();
2275 }
2276
2277
2278
2279
2280
2281
2282 public RangeIonosphericDelayModifier getRangeModifier(final Frequency frequency,
2283 final AbsoluteDate date)
2284 {
2285 final DateComponents dc = date.getComponents(TimeScalesFactory.getUTC()).getDate();
2286 ensureFrequencyAndDateSupported(frequency, dc);
2287 return rangeModifiers.get(frequency).get(dc);
2288 }
2289
2290
2291
2292
2293
2294
2295 public RangeRateIonosphericDelayModifier getRangeRateModifier(final Frequency frequency,
2296 final AbsoluteDate date)
2297 {
2298 final DateComponents dc = date.getComponents(TimeScalesFactory.getUTC()).getDate();
2299 ensureFrequencyAndDateSupported(frequency, dc);
2300 return rangeRateModifiers.get(frequency).get(dc);
2301 }
2302
2303
2304
2305
2306
2307 private void ensureFrequencyAndDateSupported(final Frequency frequency, final DateComponents dc)
2308 {
2309
2310 if (!rangeModifiers.containsKey(frequency)) {
2311 rangeModifiers.put(frequency, new HashMap<>());
2312 rangeRateModifiers.put(frequency, new HashMap<>());
2313 }
2314
2315 if (!rangeModifiers.get(frequency).containsKey(dc)) {
2316
2317
2318 final KlobucharIonoCoefficientsLoader loader = new KlobucharIonoCoefficientsLoader();
2319 loader.loadKlobucharIonosphericCoefficients(dc);
2320 final IonosphericModel l1Model = new KlobucharIonoModel(loader.getAlpha(), loader.getBeta());
2321
2322
2323 final double fL1 = Frequency.G01.getMHzFrequency();
2324 final double f = frequency.getMHzFrequency();
2325 final double ratio = (fL1 * fL1) / (f * f);
2326 final IonosphericModel scaledModel = (date, geo, elevation, azimuth) ->
2327 ratio * l1Model.pathDelay(date, geo, elevation, azimuth);
2328
2329
2330 rangeModifiers.get(frequency).put(dc, new RangeIonosphericDelayModifier(scaledModel));
2331 rangeRateModifiers.get(frequency).put(dc, new RangeRateIonosphericDelayModifier(scaledModel, twoWay));
2332
2333 }
2334
2335 }
2336
2337 }
2338
2339
2340 private static enum AttitudeMode {
2341 NADIR_POINTING_WITH_YAW_COMPENSATION() {
2342 public AttitudeProvider getProvider(final Frame inertialFrame, final OneAxisEllipsoid body)
2343 {
2344 return new YawCompensation(inertialFrame, new NadirPointing(inertialFrame, body));
2345 }
2346 },
2347 CENTER_POINTING_WITH_YAW_STEERING {
2348 public AttitudeProvider getProvider(final Frame inertialFrame, final OneAxisEllipsoid body)
2349 {
2350 return new YawSteering(inertialFrame,
2351 new BodyCenterPointing(inertialFrame, body),
2352 CelestialBodyFactory.getSun(),
2353 Vector3D.PLUS_I);
2354 }
2355 },
2356 LOF_ALIGNED_LVLH {
2357 public AttitudeProvider getProvider(final Frame inertialFrame, final OneAxisEllipsoid body)
2358 {
2359 return new LofOffset(inertialFrame, LOFType.LVLH);
2360 }
2361 },
2362 LOF_ALIGNED_QSW {
2363 public AttitudeProvider getProvider(final Frame inertialFrame, final OneAxisEllipsoid body)
2364 {
2365 return new LofOffset(inertialFrame, LOFType.QSW);
2366 }
2367 },
2368 LOF_ALIGNED_TNW {
2369 public AttitudeProvider getProvider(final Frame inertialFrame, final OneAxisEllipsoid body)
2370 {
2371 return new LofOffset(inertialFrame, LOFType.TNW);
2372 }
2373 },
2374 LOF_ALIGNED_VNC {
2375 public AttitudeProvider getProvider(final Frame inertialFrame, final OneAxisEllipsoid body)
2376 {
2377 return new LofOffset(inertialFrame, LOFType.VNC);
2378 }
2379 },
2380 LOF_ALIGNED_VVLH {
2381 public AttitudeProvider getProvider(final Frame inertialFrame, final OneAxisEllipsoid body)
2382 {
2383 return new LofOffset(inertialFrame, LOFType.VVLH);
2384 }
2385 };
2386
2387 public abstract AttitudeProvider getProvider(final Frame inertialFrame, final OneAxisEllipsoid body)
2388 ;
2389
2390 }
2391
2392
2393 private static enum ParameterKey {
2394 ORBIT_DATE,
2395 ORBIT_CIRCULAR_A,
2396 ORBIT_CIRCULAR_EX,
2397 ORBIT_CIRCULAR_EY,
2398 ORBIT_CIRCULAR_I,
2399 ORBIT_CIRCULAR_RAAN,
2400 ORBIT_CIRCULAR_ALPHA,
2401 ORBIT_EQUINOCTIAL_A,
2402 ORBIT_EQUINOCTIAL_EX,
2403 ORBIT_EQUINOCTIAL_EY,
2404 ORBIT_EQUINOCTIAL_HX,
2405 ORBIT_EQUINOCTIAL_HY,
2406 ORBIT_EQUINOCTIAL_LAMBDA,
2407 ORBIT_KEPLERIAN_A,
2408 ORBIT_KEPLERIAN_E,
2409 ORBIT_KEPLERIAN_I,
2410 ORBIT_KEPLERIAN_PA,
2411 ORBIT_KEPLERIAN_RAAN,
2412 ORBIT_KEPLERIAN_ANOMALY,
2413 ORBIT_ANGLE_TYPE,
2414 ORBIT_TLE_LINE_1,
2415 ORBIT_TLE_LINE_2,
2416 ORBIT_CARTESIAN_PX,
2417 ORBIT_CARTESIAN_PY,
2418 ORBIT_CARTESIAN_PZ,
2419 ORBIT_CARTESIAN_VX,
2420 ORBIT_CARTESIAN_VY,
2421 ORBIT_CARTESIAN_VZ,
2422 MASS,
2423 IERS_CONVENTIONS,
2424 INERTIAL_FRAME,
2425 PROPAGATOR_MIN_STEP,
2426 PROPAGATOR_MAX_STEP,
2427 PROPAGATOR_POSITION_ERROR,
2428 BODY_FRAME,
2429 BODY_EQUATORIAL_RADIUS,
2430 BODY_INVERSE_FLATTENING,
2431 CENTRAL_BODY_DEGREE,
2432 CENTRAL_BODY_ORDER,
2433 OCEAN_TIDES_DEGREE,
2434 OCEAN_TIDES_ORDER,
2435 SOLID_TIDES_SUN,
2436 SOLID_TIDES_MOON,
2437 THIRD_BODY_SUN,
2438 THIRD_BODY_MOON,
2439 DRAG,
2440 DRAG_CD,
2441 DRAG_CD_ESTIMATED,
2442 DRAG_AREA,
2443 SOLAR_RADIATION_PRESSURE,
2444 SOLAR_RADIATION_PRESSURE_CR,
2445 SOLAR_RADIATION_PRESSURE_CR_ESTIMATED,
2446 SOLAR_RADIATION_PRESSURE_AREA,
2447 GENERAL_RELATIVITY,
2448 ATTITUDE_MODE,
2449 POLYNOMIAL_ACCELERATION_NAME,
2450 POLYNOMIAL_ACCELERATION_DIRECTION_X,
2451 POLYNOMIAL_ACCELERATION_DIRECTION_Y,
2452 POLYNOMIAL_ACCELERATION_DIRECTION_Z,
2453 POLYNOMIAL_ACCELERATION_COEFFICIENTS,
2454 POLYNOMIAL_ACCELERATION_ESTIMATED,
2455 ONBOARD_RANGE_BIAS,
2456 ONBOARD_RANGE_BIAS_MIN,
2457 ONBOARD_RANGE_BIAS_MAX,
2458 ONBOARD_RANGE_BIAS_ESTIMATED,
2459 ON_BOARD_ANTENNA_PHASE_CENTER_X,
2460 ON_BOARD_ANTENNA_PHASE_CENTER_Y,
2461 ON_BOARD_ANTENNA_PHASE_CENTER_Z,
2462 ON_BOARD_CLOCK_OFFSET,
2463 ON_BOARD_CLOCK_OFFSET_MIN,
2464 ON_BOARD_CLOCK_OFFSET_MAX,
2465 ON_BOARD_CLOCK_OFFSET_ESTIMATED,
2466 GROUND_STATION_NAME,
2467 GROUND_STATION_LATITUDE,
2468 GROUND_STATION_LONGITUDE,
2469 GROUND_STATION_ALTITUDE,
2470 GROUND_STATION_CLOCK_OFFSET,
2471 GROUND_STATION_CLOCK_OFFSET_MIN,
2472 GROUND_STATION_CLOCK_OFFSET_MAX,
2473 GROUND_STATION_CLOCK_OFFSET_ESTIMATED,
2474 GROUND_STATION_POSITION_ESTIMATED,
2475 GROUND_STATION_TROPOSPHERIC_MODEL_ESTIMATED,
2476 GROUND_STATION_TROPOSPHERIC_ZENITH_DELAY,
2477 GROUND_STATION_TROPOSPHERIC_DELAY_ESTIMATED,
2478 GROUND_STATION_GLOBAL_MAPPING_FUNCTION,
2479 GROUND_STATION_NIELL_MAPPING_FUNCTION,
2480 GROUND_STATION_WEATHER_ESTIMATED,
2481 GROUND_STATION_RANGE_SIGMA,
2482 GROUND_STATION_RANGE_BIAS,
2483 GROUND_STATION_RANGE_BIAS_MIN,
2484 GROUND_STATION_RANGE_BIAS_MAX,
2485 GROUND_STATION_RANGE_BIAS_ESTIMATED,
2486 GROUND_STATION_RANGE_RATE_SIGMA,
2487 GROUND_STATION_RANGE_RATE_BIAS,
2488 GROUND_STATION_RANGE_RATE_BIAS_MIN,
2489 GROUND_STATION_RANGE_RATE_BIAS_MAX,
2490 GROUND_STATION_RANGE_RATE_BIAS_ESTIMATED,
2491 GROUND_STATION_AZIMUTH_SIGMA,
2492 GROUND_STATION_AZIMUTH_BIAS,
2493 GROUND_STATION_AZIMUTH_BIAS_MIN,
2494 GROUND_STATION_AZIMUTH_BIAS_MAX,
2495 GROUND_STATION_ELEVATION_SIGMA,
2496 GROUND_STATION_ELEVATION_BIAS,
2497 GROUND_STATION_ELEVATION_BIAS_MIN,
2498 GROUND_STATION_ELEVATION_BIAS_MAX,
2499 GROUND_STATION_AZ_EL_BIASES_ESTIMATED,
2500 GROUND_STATION_ELEVATION_REFRACTION_CORRECTION,
2501 GROUND_STATION_RANGE_TROPOSPHERIC_CORRECTION,
2502 GROUND_STATION_IONOSPHERIC_CORRECTION,
2503 SOLID_TIDES_DISPLACEMENT_CORRECTION,
2504 SOLID_TIDES_DISPLACEMENT_REMOVE_PERMANENT_DEFORMATION,
2505 OCEAN_LOADING_CORRECTION,
2506 RANGE_MEASUREMENTS_BASE_WEIGHT,
2507 RANGE_RATE_MEASUREMENTS_BASE_WEIGHT,
2508 AZIMUTH_MEASUREMENTS_BASE_WEIGHT,
2509 ELEVATION_MEASUREMENTS_BASE_WEIGHT,
2510 PV_MEASUREMENTS_BASE_WEIGHT,
2511 PV_MEASUREMENTS_POSITION_SIGMA,
2512 PV_MEASUREMENTS_VELOCITY_SIGMA,
2513 RANGE_OUTLIER_REJECTION_MULTIPLIER,
2514 RANGE_OUTLIER_REJECTION_STARTING_ITERATION,
2515 RANGE_RATE_OUTLIER_REJECTION_MULTIPLIER,
2516 RANGE_RATE_OUTLIER_REJECTION_STARTING_ITERATION,
2517 AZ_EL_OUTLIER_REJECTION_MULTIPLIER,
2518 AZ_EL_OUTLIER_REJECTION_STARTING_ITERATION,
2519 PV_OUTLIER_REJECTION_MULTIPLIER,
2520 PV_OUTLIER_REJECTION_STARTING_ITERATION,
2521 SATELLITE_ID_IN_RINEX_FILES,
2522 MEASUREMENTS_FILES,
2523 OUTPUT_BASE_NAME,
2524 ESTIMATOR_OPTIMIZATION_ENGINE,
2525 ESTIMATOR_LEVENBERG_MARQUARDT_INITIAL_STEP_BOUND_FACTOR,
2526 ESTIMATOR_ORBITAL_PARAMETERS_POSITION_SCALE,
2527 ESTIMATOR_NORMALIZED_PARAMETERS_CONVERGENCE_THRESHOLD,
2528 ESTIMATOR_MAX_ITERATIONS,
2529 ESTIMATOR_MAX_EVALUATIONS;
2530 }
2531
2532 }