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