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.estimation.leastsquares.BatchLSEstimator;
40 import org.orekit.estimation.measurements.EstimatedMeasurement;
41 import org.orekit.estimation.measurements.GroundStation;
42 import org.orekit.estimation.measurements.MeasurementCreator;
43 import org.orekit.estimation.measurements.ObservedMeasurement;
44 import org.orekit.estimation.sequential.SemiAnalyticalKalmanEstimator;
45 import org.orekit.forces.drag.IsotropicDrag;
46 import org.orekit.forces.gravity.potential.GRGSFormatReader;
47 import org.orekit.forces.gravity.potential.GravityFieldFactory;
48 import org.orekit.forces.radiation.IsotropicRadiationClassicalConvention;
49 import org.orekit.frames.EOPHistory;
50 import org.orekit.frames.FieldTransform;
51 import org.orekit.frames.Frame;
52 import org.orekit.frames.FramesFactory;
53 import org.orekit.frames.Transform;
54 import org.orekit.frames.TransformProvider;
55 import org.orekit.models.earth.displacement.StationDisplacement;
56 import org.orekit.models.earth.displacement.TidalDisplacement;
57 import org.orekit.orbits.EquinoctialOrbit;
58 import org.orekit.orbits.KeplerianOrbit;
59 import org.orekit.orbits.Orbit;
60 import org.orekit.orbits.OrbitType;
61 import org.orekit.orbits.PositionAngle;
62 import org.orekit.propagation.Propagator;
63 import org.orekit.propagation.conversion.PropagatorBuilder;
64 import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
65 import org.orekit.time.AbsoluteDate;
66 import org.orekit.time.FieldAbsoluteDate;
67 import org.orekit.time.TimeScalesFactory;
68 import org.orekit.utils.Constants;
69 import org.orekit.utils.IERSConventions;
70 import org.orekit.utils.PVCoordinates;
71 import org.orekit.utils.ParameterDriver;
72
73
74 public class DSSTEstimationTestUtils {
75
76 public static DSSTContext eccentricContext(final String dataRoot) {
77
78 Utils.setDataRoot(dataRoot);
79 DSSTContext context = new DSSTContext();
80 context.conventions = IERSConventions.IERS_2010;
81 context.earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
82 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[] {
92 new TidalDisplacement(Constants.EIGEN5C_EARTH_EQUATORIAL_RADIUS,
93 Constants.JPL_SSD_SUN_EARTH_PLUS_MOON_MASS_RATIO,
94 Constants.JPL_SSD_EARTH_MOON_MASS_RATIO,
95 context.sun, context.moon,
96 context.conventions, false)
97 };
98 GravityFieldFactory.addPotentialCoefficientsReader(new GRGSFormatReader("grim4s4_gr", true));
99 context.gravity = GravityFieldFactory.getUnnormalizedProvider(20, 20);
100
101 Orbit orbit = new KeplerianOrbit(15000000.0, 0.125, 1.25,
102 0.250, 1.375, 0.0625, PositionAngle.MEAN,
103 FramesFactory.getEME2000(),
104 new AbsoluteDate(2000, 2, 24, 11, 35, 47.0, context.utc),
105 context.gravity.getMu());
106
107 context.initialOrbit = (EquinoctialOrbit) OrbitType.EQUINOCTIAL.convertType(orbit);
108
109 context.stations = Arrays.asList(
110 context.createStation(-53.05388, -75.01551, 1750.0, "Isla Desolación"),
111 context.createStation( 62.29639, -7.01250, 880.0, "Slættaratindur")
112
113 );
114
115
116
117
118 context.TARstations = new HashMap<GroundStation, GroundStation>();
119
120 context.TARstations.put(context.createStation(-53.05388, -75.01551, 1750.0, "Isla Desolación"),
121 context.createStation(-54.815833, -68.317778, 6.0, "Ushuaïa"));
122
123 context.TARstations.put(context.createStation( 62.29639, -7.01250, 880.0, "Slættaratindur"),
124 context.createStation( 61.405833, -6.705278, 470.0, "Sumba"));
125
126 return context;
127
128 }
129
130 public static DSSTContext geoStationnaryContext(final String dataRoot) {
131
132 Utils.setDataRoot(dataRoot);
133 DSSTContext context = new DSSTContext();
134 context.conventions = IERSConventions.IERS_2010;
135 context.utc = TimeScalesFactory.getUTC();
136 context.ut1 = TimeScalesFactory.getUT1(context.conventions, true);
137 context.displacements = new StationDisplacement[0];
138 String Myframename = "MyEarthFrame";
139 final AbsoluteDate datedef = new AbsoluteDate(2000, 1, 1, 12, 0, 0.0, context.utc);
140 final double omega = Constants.WGS84_EARTH_ANGULAR_VELOCITY;
141 final Vector3D rotationRate = new Vector3D(0.0, 0.0, omega);
142
143 TransformProvider MyEarthFrame = new TransformProvider() {
144 private static final long serialVersionUID = 1L;
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 PositionAngle.MEAN,
229 orbitArray, null);
230 for (int i = 0; i < orbitArray.length; ++i) {
231 propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(i).setValue(orbitArray[i]);
232 }
233
234 return propagatorBuilder.buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
235
236 }
237
238 public static List<ObservedMeasurement<?>> createMeasurements(final Propagator propagator,
239 final MeasurementCreator creator,
240 final double startPeriod, final double endPeriod,
241 final double step) {
242
243 propagator.setStepHandler(step, creator);
244 final double period = propagator.getInitialState().getKeplerianPeriod();
245 final AbsoluteDate start = propagator.getInitialState().getDate().shiftedBy(startPeriod * period);
246 final AbsoluteDate end = propagator.getInitialState().getDate().shiftedBy(endPeriod * period);
247 propagator.propagate(start, end);
248
249 final List<ObservedMeasurement<?>> measurements = creator.getMeasurements();
250
251 for (final ObservedMeasurement<?> measurement : measurements) {
252 for (final ParameterDriver driver : measurement.getParametersDrivers()) {
253 if (driver.getReferenceDate() == null) {
254 driver.setReferenceDate(propagator.getInitialState().getDate());
255 }
256 }
257 }
258
259 return measurements;
260
261 }
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278 public static void checkFit(final DSSTContext context, final BatchLSEstimator estimator,
279 final int iterations, final int evaluations,
280 final double expectedRMS, final double rmsEps,
281 final double expectedMax, final double maxEps,
282 final double expectedDeltaPos, final double posEps,
283 final double expectedDeltaVel, final double velEps) {
284
285 final Orbit estimatedOrbit = estimator.estimate()[0].getInitialState().getOrbit();
286 final Vector3D estimatedPosition = estimatedOrbit.getPVCoordinates().getPosition();
287 final Vector3D estimatedVelocity = estimatedOrbit.getPVCoordinates().getVelocity();
288
289 Assert.assertEquals(iterations, estimator.getIterationsCount());
290 Assert.assertEquals(evaluations, estimator.getEvaluationsCount());
291 Optimum optimum = estimator.getOptimum();
292 Assert.assertEquals(iterations, optimum.getIterations());
293 Assert.assertEquals(evaluations, optimum.getEvaluations());
294
295 int k = 0;
296 double sum = 0;
297 double max = 0;
298 for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry :
299 estimator.getLastEstimations().entrySet()) {
300 final ObservedMeasurement<?> m = entry.getKey();
301 final EstimatedMeasurement<?> e = entry.getValue();
302 final double[] weight = m.getBaseWeight();
303 final double[] sigma = m.getTheoreticalStandardDeviation();
304 final double[] observed = m.getObservedValue();
305 final double[] theoretical = e.getEstimatedValue();
306 for (int i = 0; i < m.getDimension(); ++i) {
307 final double weightedResidual = weight[i] * (theoretical[i] - observed[i]) / sigma[i];
308 ++k;
309 sum += weightedResidual * weightedResidual;
310 max = FastMath.max(max, FastMath.abs(weightedResidual));
311 }
312 }
313
314 Assert.assertEquals(expectedRMS,
315 FastMath.sqrt(sum / k),
316 rmsEps);
317 Assert.assertEquals(expectedMax,
318 max,
319 maxEps);
320 Assert.assertEquals(expectedDeltaPos,
321 Vector3D.distance(context.initialOrbit.getPVCoordinates().getPosition(), estimatedPosition),
322 posEps);
323 Assert.assertEquals(expectedDeltaVel,
324 Vector3D.distance(context.initialOrbit.getPVCoordinates().getVelocity(), estimatedVelocity),
325 velEps);
326
327 }
328
329
330
331
332
333
334
335
336
337
338
339
340 public static void checkKalmanFit(final DSSTContext context, final SemiAnalyticalKalmanEstimator kalman,
341 final List<ObservedMeasurement<?>> measurements,
342 final Orbit refOrbit, final PositionAngle positionAngle,
343 final double expectedDeltaPos, final double posEps,
344 final double expectedDeltaVel, final double velEps) {
345 checkKalmanFit(context, kalman, measurements,
346 new Orbit[] { refOrbit },
347 new PositionAngle[] { positionAngle },
348 new double[] { expectedDeltaPos }, new double[] { posEps },
349 new double[] { expectedDeltaVel }, new double[] { velEps });
350 }
351
352
353
354
355
356
357
358
359
360
361
362
363 public static void checkKalmanFit(final DSSTContext context, final SemiAnalyticalKalmanEstimator kalman,
364 final List<ObservedMeasurement<?>> measurements,
365 final Orbit[] refOrbit, final PositionAngle[] positionAngle,
366 final double[] expectedDeltaPos, final double[] posEps,
367 final double[] expectedDeltaVel, final double []velEps) {
368
369
370 DSSTPropagator estimated = kalman.processMeasurements(measurements);
371
372
373 Assert.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
374
375 for (int k = 0; k < refOrbit.length; ++k) {
376
377 final Orbit estimatedOrbit = estimated.getInitialState().getOrbit();
378 final Vector3D estimatedPosition = estimatedOrbit.getPVCoordinates().getPosition();
379 final Vector3D estimatedVelocity = estimatedOrbit.getPVCoordinates().getVelocity();
380
381
382 final RealMatrix estimatedP = kalman.getPhysicalEstimatedCovarianceMatrix();
383
384
385
386 final double[][] dCdY = new double[6][6];
387 estimatedOrbit.getJacobianWrtParameters(positionAngle[k], dCdY);
388 final RealMatrix Jacobian = MatrixUtils.createRealMatrix(dCdY);
389 final RealMatrix estimatedCartesianP =
390 Jacobian.
391 multiply(estimatedP.getSubMatrix(0, 5, 0, 5)).
392 multiply(Jacobian.transpose());
393
394
395 final double[] sigmas = new double[6];
396 for (int i = 0; i < 6; i++) {
397 sigmas[i] = FastMath.sqrt(estimatedCartesianP.getEntry(i, i));
398 }
399
400
401 final double deltaPosK = Vector3D.distance(refOrbit[k].getPVCoordinates().getPosition(), estimatedPosition);
402 final double deltaVelK = Vector3D.distance(refOrbit[k].getPVCoordinates().getVelocity(), estimatedVelocity);
403 Assert.assertEquals(0.0, refOrbit[k].getDate().durationFrom(estimatedOrbit.getDate()), 1.0e-10);
404 Assert.assertEquals(expectedDeltaPos[k], deltaPosK, posEps[k]);
405 Assert.assertEquals(expectedDeltaVel[k], deltaVelK, velEps[k]);
406
407 }
408 }
409
410 }