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