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.KalmanEstimator;
45 import org.orekit.frames.EOPHistory;
46 import org.orekit.frames.FieldTransform;
47 import org.orekit.frames.Frame;
48 import org.orekit.frames.FramesFactory;
49 import org.orekit.frames.Transform;
50 import org.orekit.frames.TransformProvider;
51 import org.orekit.models.earth.displacement.StationDisplacement;
52 import org.orekit.models.earth.displacement.TidalDisplacement;
53 import org.orekit.orbits.Orbit;
54 import org.orekit.orbits.PositionAngle;
55 import org.orekit.propagation.Propagator;
56 import org.orekit.propagation.analytical.tle.TLE;
57 import org.orekit.propagation.analytical.tle.TLEPropagator;
58 import org.orekit.propagation.conversion.PropagatorBuilder;
59 import org.orekit.time.AbsoluteDate;
60 import org.orekit.time.FieldAbsoluteDate;
61 import org.orekit.time.TimeScalesFactory;
62 import org.orekit.utils.Constants;
63 import org.orekit.utils.IERSConventions;
64 import org.orekit.utils.ParameterDriver;
65
66
67 public class TLEEstimationTestUtils {
68
69 public static TLEContext eccentricContext(final String dataRoot) {
70
71 Utils.setDataRoot(dataRoot);
72 TLEContext context = new TLEContext();
73 context.conventions = IERSConventions.IERS_2010;
74 context.earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
75 Constants.WGS84_EARTH_FLATTENING,
76 FramesFactory.getITRF(context.conventions, true));
77 context.sun = CelestialBodyFactory.getSun();
78 context.moon = CelestialBodyFactory.getMoon();
79 final EOPHistory eopHistory = FramesFactory.getEOPHistory(context.conventions, true);
80 context.utc = TimeScalesFactory.getUTC();
81 context.ut1 = TimeScalesFactory.getUT1(eopHistory);
82 context.displacements = new StationDisplacement[] {
83 new TidalDisplacement(Constants.EIGEN5C_EARTH_EQUATORIAL_RADIUS,
84 Constants.JPL_SSD_SUN_EARTH_PLUS_MOON_MASS_RATIO,
85 Constants.JPL_SSD_EARTH_MOON_MASS_RATIO,
86 context.sun, context.moon,
87 context.conventions, false)
88 };
89
90 String line1 = "1 07276U 74026A 00055.48318287 .00000000 00000-0 22970+3 0 9994";
91 String line2 = "2 07276 71.6273 78.7838 1248323 14.0598 3.8405 4.72707036231812";
92 context.initialTLE = new TLE(line1, line2);
93
94 context.stations = Arrays.asList(
95 context.createStation(-53.05388, -75.01551, 1750.0, "Isla Desolación"),
96 context.createStation( 62.29639, -7.01250, 880.0, "Slættaratindur")
97
98 );
99
100
101
102
103 context.TARstations = new HashMap<GroundStation, GroundStation>();
104
105 context.TARstations.put(context.createStation(-53.05388, -75.01551, 1750.0, "Isla Desolación"),
106 context.createStation(-54.815833, -68.317778, 6.0, "Ushuaïa"));
107
108 context.TARstations.put(context.createStation( 62.29639, -7.01250, 880.0, "Slættaratindur"),
109 context.createStation( 61.405833, -6.705278, 470.0, "Sumba"));
110
111 return context;
112
113 }
114
115 public static TLEContext geoStationnaryContext(final String dataRoot) {
116
117 Utils.setDataRoot(dataRoot);
118 TLEContext context = new TLEContext();
119 context.conventions = IERSConventions.IERS_2010;
120 context.utc = TimeScalesFactory.getUTC();
121 context.ut1 = TimeScalesFactory.getUT1(context.conventions, true);
122 context.displacements = new StationDisplacement[0];
123 String Myframename = "MyEarthFrame";
124 final AbsoluteDate datedef = new AbsoluteDate(2000, 1, 1, 12, 0, 0.0, context.utc);
125 final double omega = Constants.WGS84_EARTH_ANGULAR_VELOCITY;
126 final Vector3D rotationRate = new Vector3D(0.0, 0.0, omega);
127
128 TransformProvider MyEarthFrame = new TransformProvider() {
129 private static final long serialVersionUID = 1L;
130 public Transform getTransform(final AbsoluteDate date) {
131 final double rotationduration = date.durationFrom(datedef);
132 final Vector3D alpharot = new Vector3D(rotationduration, rotationRate);
133 final Rotation rotation = new Rotation(Vector3D.PLUS_K, -alpharot.getZ(),
134 RotationConvention.VECTOR_OPERATOR);
135 return new Transform(date, rotation, rotationRate);
136 }
137 public <T extends CalculusFieldElement<T>> FieldTransform<T> getTransform(final FieldAbsoluteDate<T> date) {
138 final T rotationduration = date.durationFrom(datedef);
139 final FieldVector3D<T> alpharot = new FieldVector3D<>(rotationduration, rotationRate);
140 final FieldRotation<T> rotation = new FieldRotation<>(FieldVector3D.getPlusK(date.getField()),
141 alpharot.getZ().negate(),
142 RotationConvention.VECTOR_OPERATOR);
143 return new FieldTransform<>(date, rotation, new FieldVector3D<>(date.getField(), rotationRate));
144 }
145 };
146 Frame FrameTest = new Frame(FramesFactory.getEME2000(), MyEarthFrame, Myframename, true);
147
148
149 context.earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, 0.0, FrameTest);
150 context.sun = CelestialBodyFactory.getSun();
151 context.moon = CelestialBodyFactory.getMoon();
152
153
154
155
156 context.stations = Collections.singletonList(context.createStation(0.0, 0.0, 0.0, "Lat0_Long0") );
157
158
159 String line1 = "1 24786U 97019A 00002.84035656 .00000093 00000-0 00000-0 0 4663";
160 String line2 = "2 24786 0.0023 170.4335 0003424 130.3470 328.3614 1.00279571 9860";
161 final TLE tle = new TLE(line1, line2);
162
163
164 context.initialTLE = tle;
165
166 context.stations = Collections.singletonList(context.createStation(10.0, 45.0, 0.0, "Lat10_Long45") );
167
168
169
170
171 context.TARstations = new HashMap<GroundStation, GroundStation>();
172
173 context.TARstations.put(context.createStation( 41.977, 13.600, 671.354, "Fucino"),
174 context.createStation( 43.604, 1.444, 263.0 , "ToulouMEANse"));
175
176 context.TARstations.put(context.createStation( 49.867, 8.65 , 144.0 , "Darmstadt"),
177 context.createStation( -25.885, 27.707, 1566.633, "Pretoria"));
178
179 return context;
180
181 }
182
183 public static Propagator createPropagator(final Orbit initialOrbit,
184 final PropagatorBuilder propagatorBuilder) {
185
186
187 double[] orbitArray = new double[6];
188 initialOrbit.getType().mapOrbitToArray(initialOrbit, PositionAngle.MEAN, orbitArray, null);
189 for (int i = 0; i < orbitArray.length; ++i) {
190 propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(i).setValue(orbitArray[i]);
191 }
192
193 return propagatorBuilder.buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
194
195 }
196
197 public static List<ObservedMeasurement<?>> createMeasurements(final Propagator propagator,
198 final MeasurementCreator creator,
199 final double startPeriod, final double endPeriod,
200 final double step) {
201
202 propagator.setStepHandler(step, creator);
203 final double period = propagator.getInitialState().getKeplerianPeriod();
204 final AbsoluteDate start = propagator.getInitialState().getDate().shiftedBy(startPeriod * period);
205 final AbsoluteDate end = propagator.getInitialState().getDate().shiftedBy(endPeriod * period);
206 propagator.propagate(start, end);
207
208 final List<ObservedMeasurement<?>> measurements = creator.getMeasurements();
209
210 for (final ObservedMeasurement<?> measurement : measurements) {
211 for (final ParameterDriver driver : measurement.getParametersDrivers()) {
212 if (driver.getReferenceDate() == null) {
213 driver.setReferenceDate(propagator.getInitialState().getDate());
214 }
215 }
216 }
217
218 return measurements;
219
220 }
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237 public static void checkFit(final TLEContext context, final BatchLSEstimator estimator,
238 final int iterations, final int evaluations,
239 final double expectedRMS, final double rmsEps,
240 final double expectedMax, final double maxEps,
241 final double expectedDeltaPos, final double posEps,
242 final double expectedDeltaVel, final double velEps) {
243
244 final Orbit initialOrbit = TLEPropagator.selectExtrapolator(context.initialTLE).getInitialState().getOrbit();
245 final Orbit estimatedOrbit = estimator.estimate()[0].getInitialState().getOrbit();
246 final Vector3D estimatedPosition = estimatedOrbit.getPVCoordinates().getPosition();
247 final Vector3D estimatedVelocity = estimatedOrbit.getPVCoordinates().getVelocity();
248
249 Assert.assertEquals(iterations, estimator.getIterationsCount());
250 Assert.assertEquals(evaluations, estimator.getEvaluationsCount());
251 Optimum optimum = estimator.getOptimum();
252 Assert.assertEquals(iterations, optimum.getIterations());
253 Assert.assertEquals(evaluations, optimum.getEvaluations());
254
255 int k = 0;
256 double sum = 0;
257 double max = 0;
258 for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry :
259 estimator.getLastEstimations().entrySet()) {
260 final ObservedMeasurement<?> m = entry.getKey();
261 final EstimatedMeasurement<?> e = entry.getValue();
262 final double[] weight = m.getBaseWeight();
263 final double[] sigma = m.getTheoreticalStandardDeviation();
264 final double[] observed = m.getObservedValue();
265 final double[] theoretical = e.getEstimatedValue();
266 for (int i = 0; i < m.getDimension(); ++i) {
267 final double weightedResidual = weight[i] * (theoretical[i] - observed[i]) / sigma[i];
268 ++k;
269 sum += weightedResidual * weightedResidual;
270 max = FastMath.max(max, FastMath.abs(weightedResidual));
271 }
272 }
273
274 Assert.assertEquals(expectedRMS,
275 FastMath.sqrt(sum / k),
276 rmsEps);
277 Assert.assertEquals(expectedMax,
278 max,
279 maxEps);
280 Assert.assertEquals(expectedDeltaPos,
281 Vector3D.distance(initialOrbit.getPVCoordinates().getPosition(), estimatedPosition),
282 posEps);
283 Assert.assertEquals(expectedDeltaVel,
284 Vector3D.distance(initialOrbit.getPVCoordinates().getVelocity(), estimatedVelocity),
285 velEps);
286
287 }
288
289
290
291
292
293
294
295
296
297
298
299
300 public static void checkKalmanFit(final TLEContext context, final KalmanEstimator kalman,
301 final List<ObservedMeasurement<?>> measurements,
302 final Orbit refOrbit, final PositionAngle positionAngle,
303 final double expectedDeltaPos, final double posEps,
304 final double expectedDeltaVel, final double velEps) {
305 checkKalmanFit(context, kalman, measurements,
306 new Orbit[] { refOrbit },
307 new PositionAngle[] { positionAngle },
308 new double[] { expectedDeltaPos }, new double[] { posEps },
309 new double[] { expectedDeltaVel }, new double[] { velEps });
310 }
311
312
313
314
315
316
317
318
319
320
321
322
323 public static void checkKalmanFit(final TLEContext context, final KalmanEstimator kalman,
324 final List<ObservedMeasurement<?>> measurements,
325 final Orbit[] refOrbit, final PositionAngle[] positionAngle,
326 final double[] expectedDeltaPos, final double[] posEps,
327 final double[] expectedDeltaVel, final double []velEps) {
328
329
330 Propagator[] estimated = kalman.processMeasurements(measurements);
331
332
333 Assert.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
334
335 for (int k = 0; k < refOrbit.length; ++k) {
336
337 final Orbit estimatedOrbit = estimated[k].getInitialState().getOrbit();
338 final Vector3D estimatedPosition = estimatedOrbit.getPVCoordinates().getPosition();
339 final Vector3D estimatedVelocity = estimatedOrbit.getPVCoordinates().getVelocity();
340
341
342 final RealMatrix estimatedP = kalman.getPhysicalEstimatedCovarianceMatrix();
343
344
345
346 final double[][] dCdY = new double[6][6];
347 estimatedOrbit.getJacobianWrtParameters(positionAngle[k], dCdY);
348 final RealMatrix Jacobian = MatrixUtils.createRealMatrix(dCdY);
349 final RealMatrix estimatedCartesianP =
350 Jacobian.
351 multiply(estimatedP.getSubMatrix(0, 5, 0, 5)).
352 multiply(Jacobian.transpose());
353
354
355 final double[] sigmas = new double[6];
356 for (int i = 0; i < 6; i++) {
357 sigmas[i] = FastMath.sqrt(estimatedCartesianP.getEntry(i, i));
358 }
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374 final double deltaPosK = Vector3D.distance(refOrbit[k].getPVCoordinates().getPosition(), estimatedPosition);
375 final double deltaVelK = Vector3D.distance(refOrbit[k].getPVCoordinates().getVelocity(), estimatedVelocity);
376 Assert.assertEquals(expectedDeltaPos[k], deltaPosK, posEps[k]);
377 Assert.assertEquals(expectedDeltaVel[k], deltaVelK, velEps[k]);
378
379 }
380 }
381 }