1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation;
18
19 import java.util.Arrays;
20 import java.util.Collections;
21 import java.util.HashMap;
22 import java.util.List;
23 import java.util.Map;
24
25 import org.hipparchus.CalculusFieldElement;
26 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
27 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
28 import org.hipparchus.geometry.euclidean.threed.Rotation;
29 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
30 import org.hipparchus.geometry.euclidean.threed.Vector3D;
31 import org.hipparchus.linear.MatrixUtils;
32 import org.hipparchus.linear.RealMatrix;
33 import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer.Optimum;
34 import org.hipparchus.util.FastMath;
35 import org.hipparchus.util.Pair;
36 import org.junit.jupiter.api.Assertions;
37 import org.orekit.Utils;
38 import org.orekit.bodies.CelestialBodyFactory;
39 import org.orekit.bodies.OneAxisEllipsoid;
40 import org.orekit.data.DataContext;
41 import org.orekit.estimation.leastsquares.BatchLSEstimator;
42 import org.orekit.estimation.measurements.EstimatedMeasurement;
43 import org.orekit.estimation.measurements.MeasurementCreator;
44 import org.orekit.estimation.measurements.ObservedMeasurement;
45 import org.orekit.estimation.sequential.KalmanEstimator;
46 import org.orekit.forces.drag.IsotropicDrag;
47 import org.orekit.forces.gravity.potential.AstronomicalAmplitudeReader;
48 import org.orekit.forces.gravity.potential.FESCHatEpsilonReader;
49 import org.orekit.forces.gravity.potential.GRGSFormatReader;
50 import org.orekit.forces.gravity.potential.GravityFieldFactory;
51 import org.orekit.forces.gravity.potential.OceanLoadDeformationCoefficients;
52 import org.orekit.forces.radiation.IsotropicRadiationClassicalConvention;
53 import org.orekit.frames.EOPHistory;
54 import org.orekit.frames.FieldTransform;
55 import org.orekit.frames.Frame;
56 import org.orekit.frames.FramesFactory;
57 import org.orekit.frames.TopocentricFrame;
58 import org.orekit.frames.Transform;
59 import org.orekit.frames.TransformProvider;
60 import org.orekit.models.earth.displacement.StationDisplacement;
61 import org.orekit.models.earth.displacement.TidalDisplacement;
62 import org.orekit.orbits.KeplerianOrbit;
63 import org.orekit.orbits.Orbit;
64 import org.orekit.orbits.PositionAngleType;
65 import org.orekit.propagation.Propagator;
66 import org.orekit.propagation.conversion.PropagatorBuilder;
67 import org.orekit.propagation.events.AbstractDetector;
68 import org.orekit.propagation.events.ElevationDetector;
69 import org.orekit.propagation.events.intervals.ElevationDetectionAdaptableIntervalFactory;
70 import org.orekit.time.AbsoluteDate;
71 import org.orekit.time.FieldAbsoluteDate;
72 import org.orekit.time.TimeScalesFactory;
73 import org.orekit.utils.Constants;
74 import org.orekit.utils.IERSConventions;
75 import org.orekit.utils.PVCoordinates;
76 import org.orekit.utils.ParameterDriver;
77
78
79 public class EstimationTestUtils {
80
81 public static Context eccentricContext(final String dataRoot) {
82
83 Utils.setDataRoot(dataRoot);
84 Context context = new Context();
85 context.conventions = IERSConventions.IERS_2010;
86 context.earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
87 Constants.WGS84_EARTH_FLATTENING,
88 FramesFactory.getITRF(context.conventions, true));
89 context.sun = CelestialBodyFactory.getSun();
90 context.moon = CelestialBodyFactory.getMoon();
91 context.radiationSensitive = new IsotropicRadiationClassicalConvention(2.0, 0.2, 0.8);
92 context.dragSensitive = new IsotropicDrag(2.0, 1.2);
93 final EOPHistory eopHistory = FramesFactory.getEOPHistory(context.conventions, true);
94 context.utc = TimeScalesFactory.getUTC();
95 context.ut1 = TimeScalesFactory.getUT1(eopHistory);
96 context.displacements = new StationDisplacement[] {
97 new TidalDisplacement(Constants.EIGEN5C_EARTH_EQUATORIAL_RADIUS,
98 Constants.JPL_SSD_SUN_EARTH_PLUS_MOON_MASS_RATIO,
99 Constants.JPL_SSD_EARTH_MOON_MASS_RATIO,
100 context.sun, context.moon,
101 context.conventions, false)
102 };
103 GravityFieldFactory.addPotentialCoefficientsReader(new GRGSFormatReader("grim4s4_gr", true));
104 AstronomicalAmplitudeReader aaReader =
105 new AstronomicalAmplitudeReader("hf-fes2004.dat", 5, 2, 3, 1.0);
106 DataContext.getDefault().getDataProvidersManager().feed(aaReader.getSupportedNames(), aaReader);
107 Map<Integer, Double> map = aaReader.getAstronomicalAmplitudesMap();
108 GravityFieldFactory.addOceanTidesReader(new FESCHatEpsilonReader("fes2004-7x7.dat",
109 0.01, FastMath.toRadians(1.0),
110 OceanLoadDeformationCoefficients.IERS_2010,
111 map));
112 context.gravity = GravityFieldFactory.getNormalizedProvider(20, 20);
113 context.initialOrbit = new KeplerianOrbit(15000000.0, 0.125, 1.25,
114 0.250, 1.375, 0.0625, PositionAngleType.TRUE,
115 FramesFactory.getEME2000(),
116 new AbsoluteDate(2000, 2, 24, 11, 35, 47.0, context.utc),
117 context.gravity.getMu());
118
119 context.stations = Arrays.asList(
120 context.createStation(-53.05388, -75.01551, 1750.0, "Isla Desolación"),
121 context.createStation( 62.29639, -7.01250, 880.0, "Slættaratindur")
122
123 );
124
125
126
127
128 context.TARstations = new HashMap<>();
129
130 context.TARstations.put(context.createStation(-53.05388, -75.01551, 1750.0, "Isla Desolación"),
131 context.createStation(-54.815833, -68.317778, 6.0, "Ushuaïa"));
132
133 context.TARstations.put(context.createStation( 62.29639, -7.01250, 880.0, "Slættaratindur"),
134 context.createStation( 61.405833, -6.705278, 470.0, "Sumba"));
135
136
137
138
139 context.BRRstations = new Pair<>(context.createStation(40.0, 0.0, 0.0, "Emitter"),
140 context.createStation(45.0, 0.0, 0.0, "Receiver"));
141
142
143
144
145 context.TDOAstations = new Pair<>(context.createStation(40.0, 0.0, 0.0, "TDOA_Prime"),
146 context.createStation(45.0, 0.0, 0.0, "TDOA_Second"));
147
148
149
150
151 context.FDOAstations = context.TDOAstations;
152
153 return context;
154
155 }
156
157 public static Context geoStationnaryContext(final String dataRoot) {
158
159 Utils.setDataRoot(dataRoot);
160 Context context = new Context();
161 context.conventions = IERSConventions.IERS_2010;
162 context.utc = TimeScalesFactory.getUTC();
163 context.ut1 = TimeScalesFactory.getUT1(context.conventions, true);
164 context.displacements = new StationDisplacement[0];
165 String Myframename = "MyEarthFrame";
166 final AbsoluteDate datedef = new AbsoluteDate(2000, 1, 1, 12, 0, 0.0, context.utc);
167 final double omega = Constants.WGS84_EARTH_ANGULAR_VELOCITY;
168 final Vector3D rotationRate = new Vector3D(0.0, 0.0, omega);
169
170 TransformProvider MyEarthFrame = new TransformProvider() {
171 public Transform getTransform(final AbsoluteDate date) {
172 final double rotationduration = date.durationFrom(datedef);
173 final Vector3D alpharot = new Vector3D(rotationduration, rotationRate);
174 final Rotation rotation = new Rotation(Vector3D.PLUS_K, -alpharot.getZ(),
175 RotationConvention.VECTOR_OPERATOR);
176 return new Transform(date, rotation, rotationRate);
177 }
178 public <T extends CalculusFieldElement<T>> FieldTransform<T> getTransform(final FieldAbsoluteDate<T> date) {
179 final T rotationduration = date.durationFrom(datedef);
180 final FieldVector3D<T> alpharot = new FieldVector3D<>(rotationduration, rotationRate);
181 final FieldRotation<T> rotation = new FieldRotation<>(FieldVector3D.getPlusK(date.getField()),
182 alpharot.getZ().negate(),
183 RotationConvention.VECTOR_OPERATOR);
184 return new FieldTransform<>(date, rotation, new FieldVector3D<>(date.getField(), rotationRate));
185 }
186 };
187 Frame FrameTest = new Frame(FramesFactory.getEME2000(), MyEarthFrame, Myframename, true);
188
189
190 context.earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, 0.0, FrameTest);
191 context.sun = CelestialBodyFactory.getSun();
192 context.moon = CelestialBodyFactory.getMoon();
193 context.radiationSensitive = new IsotropicRadiationClassicalConvention(2.0, 0.2, 0.8);
194 context.dragSensitive = new IsotropicDrag(2.0, 1.2);
195 GravityFieldFactory.addPotentialCoefficientsReader(new GRGSFormatReader("grim4s4_gr", true));
196 AstronomicalAmplitudeReader aaReader =
197 new AstronomicalAmplitudeReader("hf-fes2004.dat", 5, 2, 3, 1.0);
198 DataContext.getDefault().getDataProvidersManager().feed(aaReader.getSupportedNames(), aaReader);
199 Map<Integer, Double> map = aaReader.getAstronomicalAmplitudesMap();
200 GravityFieldFactory.addOceanTidesReader(new FESCHatEpsilonReader("fes2004-7x7.dat",
201 0.01, FastMath.toRadians(1.0),
202 OceanLoadDeformationCoefficients.IERS_2010,
203 map));
204 context.gravity = GravityFieldFactory.getNormalizedProvider(20, 20);
205
206
207 double da = FastMath.cbrt(context.gravity.getMu() / (omega * omega));
208
209
210
211
212 context.stations = Collections.singletonList(context.createStation(0.0, 0.0, 0.0, "Lat0_Long0") );
213
214
215 final Vector3D geovelocity = new Vector3D (0., 0., 0.);
216
217
218 Transform topoToEME =
219 context.stations.get(0).getBaseFrame().getTransformTo(FramesFactory.getEME2000(), new AbsoluteDate(2000, 1, 1, 12, 0, 0.0, context.utc));
220
221
222 Vector3D stationPositionEME = topoToEME.transformPosition(Vector3D.ZERO);
223
224
225 final Vector3D sat_pos = new Vector3D(0., 0., da-stationPositionEME.getNorm());
226 final Vector3D acceleration = new Vector3D(-context.gravity.getMu(), sat_pos);
227 final PVCoordinates pv_sat_topo = new PVCoordinates(sat_pos, geovelocity, acceleration);
228
229
230 final PVCoordinates pv_sat_iner = topoToEME.transformPVCoordinates(pv_sat_topo);
231
232
233 context.initialOrbit = new KeplerianOrbit(pv_sat_iner,
234 FramesFactory.getEME2000(),
235 new AbsoluteDate(2000, 1, 1, 12, 0, 0.0, context.utc),
236 context.gravity.getMu());
237
238 context.stations = Collections.singletonList(context.createStation(10.0, 45.0, 0.0, "Lat10_Long45") );
239
240
241
242
243 context.TARstations = new HashMap<>();
244
245 context.TARstations.put(context.createStation( 41.977, 13.600, 671.354, "Fucino"),
246 context.createStation( 43.604, 1.444, 263.0 , "Toulouse"));
247
248 context.TARstations.put(context.createStation( 49.867, 8.65 , 144.0 , "Darmstadt"),
249 context.createStation( -25.885, 27.707, 1566.633, "Pretoria"));
250
251
252
253
254 context.BRRstations = new Pair<>(context.createStation(40.0, 0.0, 0.0, "Emitter"),
255 context.createStation(45.0, 0.0, 0.0, "Receiver"));
256
257
258
259
260 context.TDOAstations = new Pair<>(context.createStation(40.0, 0.0, 0.0, "TDOA_Prime"),
261 context.createStation(45.0, 0.0, 0.0, "TDOA_Second"));
262
263 return context;
264
265 }
266
267 public static Propagator createPropagator(final Orbit initialOrbit,
268 final PropagatorBuilder propagatorBuilder) {
269
270
271 double[] orbitArray = new double[6];
272 propagatorBuilder.getOrbitType().mapOrbitToArray(initialOrbit,
273 propagatorBuilder.getPositionAngleType(),
274 orbitArray, null);
275 for (int i = 0; i < orbitArray.length; ++i) {
276
277 propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(i).setValue(orbitArray[i]);
278 }
279
280 return propagatorBuilder.buildPropagator();
281
282 }
283
284 public static List<ObservedMeasurement<?>> createMeasurements(final Propagator propagator,
285 final MeasurementCreator creator,
286 final double startPeriod, final double endPeriod,
287 final double step) {
288
289 propagator.setStepHandler(step, creator);
290 final double period = propagator.getInitialState().getOrbit().getKeplerianPeriod();
291 final AbsoluteDate start = propagator.getInitialState().getDate().shiftedBy(startPeriod * period);
292 final AbsoluteDate end = propagator.getInitialState().getDate().shiftedBy(endPeriod * period);
293 propagator.propagate(start, end);
294
295 final List<ObservedMeasurement<?>> measurements = creator.getMeasurements();
296
297 for (final ObservedMeasurement<?> measurement : measurements) {
298 for (final ParameterDriver driver : measurement.getParametersDrivers()) {
299 if (driver.getReferenceDate() == null) {
300 driver.setReferenceDate(propagator.getInitialState().getDate());
301 }
302 }
303 }
304
305 return measurements;
306
307 }
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324 public static void checkFit(final Context context, final BatchLSEstimator estimator,
325 final int iterations, final int evaluations,
326 final double expectedRMS, final double rmsEps,
327 final double expectedMax, final double maxEps,
328 final double expectedDeltaPos, final double posEps,
329 final double expectedDeltaVel, final double velEps) {
330
331 final Orbit estimatedOrbit = estimator.estimate()[0].getInitialState().getOrbit();
332 final Vector3D estimatedPosition = estimatedOrbit.getPosition();
333 final Vector3D estimatedVelocity = estimatedOrbit.getVelocity();
334
335 int k = 0;
336 double sum = 0;
337 double max = 0;
338 for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry :
339 estimator.getLastEstimations().entrySet()) {
340 final ObservedMeasurement<?> m = entry.getKey();
341 final EstimatedMeasurement<?> e = entry.getValue();
342 final double[] weight = m.getBaseWeight();
343 final double[] sigma = m.getTheoreticalStandardDeviation();
344 final double[] observed = m.getObservedValue();
345 final double[] theoretical = e.getEstimatedValue();
346 for (int i = 0; i < m.getDimension(); ++i) {
347 final double weightedResidual = weight[i] * (theoretical[i] - observed[i]) / sigma[i];
348 ++k;
349 sum += weightedResidual * weightedResidual;
350 max = FastMath.max(max, FastMath.abs(weightedResidual));
351 }
352 }
353
354 final double rms = FastMath.sqrt(sum / k);
355 final double deltaPos = Vector3D.distance(context.initialOrbit.getPosition(), estimatedPosition);
356 final double deltaVel = Vector3D.distance(context.initialOrbit.getVelocity(), estimatedVelocity);
357
358 boolean print = false;
359 if (print) {
360 System.out.format("%25s %25s %25s %25s\n", "", "Actual", "Expected", "Tolerance");
361 System.out.format("%25s %25d %25d %25d\n", "iterations", estimator.getIterationsCount(), iterations, 0);
362 System.out.format("%25s %25d %25d %25d\n", "evaluations", estimator.getEvaluationsCount(), evaluations, 0);
363 System.out.format("%25s %25e %25e %25e\n", "RMS", rms, expectedRMS, rmsEps);
364 }
365
366 Assertions.assertEquals(expectedRMS, rms, rmsEps);
367 Assertions.assertEquals(expectedMax, max, maxEps);
368 Assertions.assertEquals(expectedDeltaPos, deltaPos, posEps);
369 Assertions.assertEquals(expectedDeltaVel, deltaVel, velEps);
370
371 Assertions.assertEquals(iterations, estimator.getIterationsCount());
372 Assertions.assertEquals(evaluations, estimator.getEvaluationsCount());
373 Optimum optimum = estimator.getOptimum();
374 Assertions.assertEquals(iterations, optimum.getIterations());
375 Assertions.assertEquals(evaluations, optimum.getEvaluations());
376 }
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393 public static void checkKalmanFit(final Context context, final KalmanEstimator kalman,
394 final List<ObservedMeasurement<?>> measurements,
395 final Orbit refOrbit, final PositionAngleType positionAngleType,
396 final double expectedDeltaPos, final double posEps,
397 final double expectedDeltaVel, final double velEps,
398 final double[] expectedSigmasPos,final double sigmaPosEps,
399 final double[] expectedSigmasVel,final double sigmaVelEps)
400 {
401 checkKalmanFit(context, kalman, measurements,
402 new Orbit[] { refOrbit },
403 new PositionAngleType[] {positionAngleType},
404 new double[] { expectedDeltaPos }, new double[] { posEps },
405 new double[] { expectedDeltaVel }, new double[] { velEps },
406 new double[][] { expectedSigmasPos }, new double[] { sigmaPosEps },
407 new double[][] { expectedSigmasVel }, new double[] { sigmaVelEps });
408 }
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425 public static void checkKalmanFit(final Context context, final KalmanEstimator kalman,
426 final List<ObservedMeasurement<?>> measurements,
427 final Orbit[] refOrbit, final PositionAngleType[] positionAngleType,
428 final double[] expectedDeltaPos, final double[] posEps,
429 final double[] expectedDeltaVel, final double []velEps,
430 final double[][] expectedSigmasPos,final double[] sigmaPosEps,
431 final double[][] expectedSigmasVel,final double[] sigmaVelEps)
432 {
433
434
435 Propagator[] estimated = kalman.processMeasurements(measurements);
436
437
438 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
439
440 for (int k = 0; k < refOrbit.length; ++k) {
441
442 final Orbit estimatedOrbit = estimated[k].getInitialState().getOrbit();
443 final Vector3D estimatedPosition = estimatedOrbit.getPosition();
444 final Vector3D estimatedVelocity = estimatedOrbit.getVelocity();
445
446
447 final RealMatrix estimatedP = kalman.getPhysicalEstimatedCovarianceMatrix();
448
449
450
451 final double[][] dCdY = new double[6][6];
452 estimatedOrbit.getJacobianWrtParameters(positionAngleType[k], dCdY);
453 final RealMatrix Jacobian = MatrixUtils.createRealMatrix(dCdY);
454 final RealMatrix estimatedCartesianP =
455 Jacobian.
456 multiply(estimatedP.getSubMatrix(0, 5, 0, 5)).
457 multiply(Jacobian.transpose());
458
459
460 final double[] sigmas = new double[6];
461 for (int i = 0; i < 6; i++) {
462 sigmas[i] = FastMath.sqrt(estimatedCartesianP.getEntry(i, i));
463 }
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479 final double deltaPosK = Vector3D.distance(refOrbit[k].getPosition(), estimatedPosition);
480 final double deltaVelK = Vector3D.distance(refOrbit[k].getVelocity(), estimatedVelocity);
481 Assertions.assertEquals(expectedDeltaPos[k], deltaPosK, posEps[k]);
482 Assertions.assertEquals(expectedDeltaVel[k], deltaVelK, velEps[k]);
483
484 for (int i = 0; i < 3; i++) {
485 Assertions.assertEquals(expectedSigmasPos[k][i], sigmas[i], sigmaPosEps[k]);
486 Assertions.assertEquals(expectedSigmasVel[k][i], sigmas[i+3], sigmaVelEps[k]);
487 }
488 }
489 }
490
491
492
493
494
495
496 public static ElevationDetector getElevationDetector(final TopocentricFrame topo, final double minElevation) {
497 ElevationDetector detector =
498 new ElevationDetector(topo).
499 withThreshold(AbstractDetector.DEFAULT_THRESHOLD).
500 withMaxCheck(ElevationDetectionAdaptableIntervalFactory.getAdaptableInterval(topo,
501 ElevationDetectionAdaptableIntervalFactory.DEFAULT_ELEVATION_SWITCH_INF,
502 ElevationDetectionAdaptableIntervalFactory.DEFAULT_ELEVATION_SWITCH_SUP,
503 10.0)).
504 withConstantElevation(minElevation);
505 return detector;
506 }
507
508 }
509