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