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.getPVCoordinates().getVelocity();
334
335 Assertions.assertEquals(iterations, estimator.getIterationsCount());
336 Assertions.assertEquals(evaluations, estimator.getEvaluationsCount());
337 Optimum optimum = estimator.getOptimum();
338 Assertions.assertEquals(iterations, optimum.getIterations());
339 Assertions.assertEquals(evaluations, optimum.getEvaluations());
340
341 int k = 0;
342 double sum = 0;
343 double max = 0;
344 for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry :
345 estimator.getLastEstimations().entrySet()) {
346 final ObservedMeasurement<?> m = entry.getKey();
347 final EstimatedMeasurement<?> e = entry.getValue();
348 final double[] weight = m.getBaseWeight();
349 final double[] sigma = m.getTheoreticalStandardDeviation();
350 final double[] observed = m.getObservedValue();
351 final double[] theoretical = e.getEstimatedValue();
352 for (int i = 0; i < m.getDimension(); ++i) {
353 final double weightedResidual = weight[i] * (theoretical[i] - observed[i]) / sigma[i];
354 ++k;
355 sum += weightedResidual * weightedResidual;
356 max = FastMath.max(max, FastMath.abs(weightedResidual));
357 }
358 }
359
360 final double rms = FastMath.sqrt(sum / k);
361 final double deltaPos = Vector3D.distance(context.initialOrbit.getPosition(), estimatedPosition);
362 final double deltaVel = Vector3D.distance(context.initialOrbit.getPVCoordinates().getVelocity(), estimatedVelocity);
363 Assertions.assertEquals(expectedRMS, rms, rmsEps);
364 Assertions.assertEquals(expectedMax, max, maxEps);
365 Assertions.assertEquals(expectedDeltaPos, deltaPos, posEps);
366 Assertions.assertEquals(expectedDeltaVel, deltaVel, velEps);
367
368 }
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385 public static void checkKalmanFit(final Context context, final KalmanEstimator kalman,
386 final List<ObservedMeasurement<?>> measurements,
387 final Orbit refOrbit, final PositionAngleType positionAngleType,
388 final double expectedDeltaPos, final double posEps,
389 final double expectedDeltaVel, final double velEps,
390 final double[] expectedSigmasPos,final double sigmaPosEps,
391 final double[] expectedSigmasVel,final double sigmaVelEps)
392 {
393 checkKalmanFit(context, kalman, measurements,
394 new Orbit[] { refOrbit },
395 new PositionAngleType[] {positionAngleType},
396 new double[] { expectedDeltaPos }, new double[] { posEps },
397 new double[] { expectedDeltaVel }, new double[] { velEps },
398 new double[][] { expectedSigmasPos }, new double[] { sigmaPosEps },
399 new double[][] { expectedSigmasVel }, new double[] { sigmaVelEps });
400 }
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417 public static void checkKalmanFit(final Context context, final KalmanEstimator kalman,
418 final List<ObservedMeasurement<?>> measurements,
419 final Orbit[] refOrbit, final PositionAngleType[] positionAngleType,
420 final double[] expectedDeltaPos, final double[] posEps,
421 final double[] expectedDeltaVel, final double []velEps,
422 final double[][] expectedSigmasPos,final double[] sigmaPosEps,
423 final double[][] expectedSigmasVel,final double[] sigmaVelEps)
424 {
425
426
427 Propagator[] estimated = kalman.processMeasurements(measurements);
428
429
430 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
431
432 for (int k = 0; k < refOrbit.length; ++k) {
433
434 final Orbit estimatedOrbit = estimated[k].getInitialState().getOrbit();
435 final Vector3D estimatedPosition = estimatedOrbit.getPosition();
436 final Vector3D estimatedVelocity = estimatedOrbit.getPVCoordinates().getVelocity();
437
438
439 final RealMatrix estimatedP = kalman.getPhysicalEstimatedCovarianceMatrix();
440
441
442
443 final double[][] dCdY = new double[6][6];
444 estimatedOrbit.getJacobianWrtParameters(positionAngleType[k], dCdY);
445 final RealMatrix Jacobian = MatrixUtils.createRealMatrix(dCdY);
446 final RealMatrix estimatedCartesianP =
447 Jacobian.
448 multiply(estimatedP.getSubMatrix(0, 5, 0, 5)).
449 multiply(Jacobian.transpose());
450
451
452 final double[] sigmas = new double[6];
453 for (int i = 0; i < 6; i++) {
454 sigmas[i] = FastMath.sqrt(estimatedCartesianP.getEntry(i, i));
455 }
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471 final double deltaPosK = Vector3D.distance(refOrbit[k].getPosition(), estimatedPosition);
472 final double deltaVelK = Vector3D.distance(refOrbit[k].getPVCoordinates().getVelocity(), estimatedVelocity);
473 Assertions.assertEquals(expectedDeltaPos[k], deltaPosK, posEps[k]);
474 Assertions.assertEquals(expectedDeltaVel[k], deltaVelK, velEps[k]);
475
476 for (int i = 0; i < 3; i++) {
477 Assertions.assertEquals(expectedSigmasPos[k][i], sigmas[i], sigmaPosEps[k]);
478 Assertions.assertEquals(expectedSigmasVel[k][i], sigmas[i+3], sigmaVelEps[k]);
479 }
480 }
481 }
482
483
484
485
486
487
488 public static ElevationDetector getElevationDetector(final TopocentricFrame topo, final double minElevation) {
489 ElevationDetector detector =
490 new ElevationDetector(topo).
491 withThreshold(AbstractDetector.DEFAULT_THRESHOLD).
492 withMaxCheck(ElevationDetectionAdaptableIntervalFactory.getAdaptableInterval(topo,
493 ElevationDetectionAdaptableIntervalFactory.DEFAULT_ELEVATION_SWITCH_INF,
494 ElevationDetectionAdaptableIntervalFactory.DEFAULT_ELEVATION_SWITCH_SUP,
495 10.0)).
496 withConstantElevation(minElevation);
497 return detector;
498 }
499
500 }
501