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.KalmanEstimator;
39 import org.orekit.frames.EOPHistory;
40 import org.orekit.frames.FieldTransform;
41 import org.orekit.frames.Frame;
42 import org.orekit.frames.FramesFactory;
43 import org.orekit.frames.Transform;
44 import org.orekit.frames.TransformProvider;
45 import org.orekit.models.earth.displacement.StationDisplacement;
46 import org.orekit.models.earth.displacement.TidalDisplacement;
47 import org.orekit.orbits.Orbit;
48 import org.orekit.orbits.PositionAngleType;
49 import org.orekit.propagation.Propagator;
50 import org.orekit.propagation.analytical.tle.TLE;
51 import org.orekit.propagation.analytical.tle.TLEPropagator;
52 import org.orekit.propagation.conversion.PropagatorBuilder;
53 import org.orekit.time.AbsoluteDate;
54 import org.orekit.time.FieldAbsoluteDate;
55 import org.orekit.time.TimeScalesFactory;
56 import org.orekit.utils.Constants;
57 import org.orekit.utils.IERSConventions;
58 import org.orekit.utils.ParameterDriver;
59
60 import java.util.Arrays;
61 import java.util.Collections;
62 import java.util.HashMap;
63 import java.util.List;
64 import java.util.Map;
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 public Transform getTransform(final AbsoluteDate date) {
130 final double rotationduration = date.durationFrom(datedef);
131 final Vector3D alpharot = new Vector3D(rotationduration, rotationRate);
132 final Rotation rotation = new Rotation(Vector3D.PLUS_K, -alpharot.getZ(),
133 RotationConvention.VECTOR_OPERATOR);
134 return new Transform(date, rotation, rotationRate);
135 }
136 public <T extends CalculusFieldElement<T>> FieldTransform<T> getTransform(final FieldAbsoluteDate<T> date) {
137 final T rotationduration = date.durationFrom(datedef);
138 final FieldVector3D<T> alpharot = new FieldVector3D<>(rotationduration, rotationRate);
139 final FieldRotation<T> rotation = new FieldRotation<>(FieldVector3D.getPlusK(date.getField()),
140 alpharot.getZ().negate(),
141 RotationConvention.VECTOR_OPERATOR);
142 return new FieldTransform<>(date, rotation, new FieldVector3D<>(date.getField(), rotationRate));
143 }
144 };
145 Frame FrameTest = new Frame(FramesFactory.getEME2000(), MyEarthFrame, Myframename, true);
146
147
148 context.earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, 0.0, FrameTest);
149 context.sun = CelestialBodyFactory.getSun();
150 context.moon = CelestialBodyFactory.getMoon();
151
152
153
154
155 context.stations = Collections.singletonList(context.createStation(0.0, 0.0, 0.0, "Lat0_Long0") );
156
157
158 String line1 = "1 24786U 97019A 00002.84035656 .00000093 00000-0 00000-0 0 4663";
159 String line2 = "2 24786 0.0023 170.4335 0003424 130.3470 328.3614 1.00279571 9860";
160 final TLE tle = new TLE(line1, line2);
161
162
163 context.initialTLE = tle;
164
165 context.stations = Collections.singletonList(context.createStation(10.0, 45.0, 0.0, "Lat10_Long45") );
166
167
168
169
170 context.TARstations = new HashMap<GroundStation, GroundStation>();
171
172 context.TARstations.put(context.createStation( 41.977, 13.600, 671.354, "Fucino"),
173 context.createStation( 43.604, 1.444, 263.0 , "ToulouMEANse"));
174
175 context.TARstations.put(context.createStation( 49.867, 8.65 , 144.0 , "Darmstadt"),
176 context.createStation( -25.885, 27.707, 1566.633, "Pretoria"));
177
178 return context;
179
180 }
181
182 public static Propagator createPropagator(final Orbit initialOrbit,
183 final PropagatorBuilder propagatorBuilder) {
184
185
186 double[] orbitArray = new double[6];
187 initialOrbit.getType().mapOrbitToArray(initialOrbit, PositionAngleType.MEAN, orbitArray, null);
188 for (int i = 0; i < orbitArray.length; ++i) {
189
190 propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(i).setValue(orbitArray[i], initialOrbit.getDate());
191 }
192
193 return propagatorBuilder.buildPropagator();
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().getOrbit().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.getPosition();
247 final Vector3D estimatedVelocity = estimatedOrbit.getPVCoordinates().getVelocity();
248
249 Assertions.assertEquals(iterations, estimator.getIterationsCount());
250 Assertions.assertEquals(evaluations, estimator.getEvaluationsCount());
251 Optimum optimum = estimator.getOptimum();
252 Assertions.assertEquals(iterations, optimum.getIterations());
253 Assertions.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 Assertions.assertEquals(expectedRMS,
275 FastMath.sqrt(sum / k),
276 rmsEps);
277 Assertions.assertEquals(expectedMax,
278 max,
279 maxEps);
280 Assertions.assertEquals(expectedDeltaPos,
281 Vector3D.distance(initialOrbit.getPosition(), estimatedPosition),
282 posEps);
283 Assertions.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 PositionAngleType positionAngleType,
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 PositionAngleType[] {positionAngleType},
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 PositionAngleType[] positionAngleType,
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 Assertions.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.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(positionAngleType[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].getPosition(), estimatedPosition);
375 final double deltaVelK = Vector3D.distance(refOrbit[k].getPVCoordinates().getVelocity(), estimatedVelocity);
376 Assertions.assertEquals(expectedDeltaPos[k], deltaPosK, posEps[k]);
377 Assertions.assertEquals(expectedDeltaVel[k], deltaVelK, velEps[k]);
378
379 }
380 }
381 }