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.geometry.euclidean.threed.Vector3D;
20 import org.hipparchus.linear.MatrixUtils;
21 import org.hipparchus.linear.RealMatrix;
22 import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer.Optimum;
23 import org.hipparchus.util.FastMath;
24 import org.junit.jupiter.api.Assertions;
25 import org.orekit.Utils;
26 import org.orekit.bodies.CelestialBodyFactory;
27 import org.orekit.bodies.OneAxisEllipsoid;
28 import org.orekit.data.DataContext;
29 import org.orekit.estimation.leastsquares.BatchLSEstimator;
30 import org.orekit.estimation.measurements.EstimatedMeasurement;
31 import org.orekit.estimation.measurements.GroundStation;
32 import org.orekit.estimation.measurements.MeasurementCreator;
33 import org.orekit.estimation.measurements.ObservedMeasurement;
34 import org.orekit.estimation.sequential.KalmanEstimator;
35 import org.orekit.forces.gravity.potential.AstronomicalAmplitudeReader;
36 import org.orekit.forces.gravity.potential.FESCHatEpsilonReader;
37 import org.orekit.forces.gravity.potential.GRGSFormatReader;
38 import org.orekit.forces.gravity.potential.GravityFieldFactory;
39 import org.orekit.forces.gravity.potential.OceanLoadDeformationCoefficients;
40 import org.orekit.frames.EOPHistory;
41 import org.orekit.frames.FramesFactory;
42 import org.orekit.models.earth.displacement.StationDisplacement;
43 import org.orekit.models.earth.displacement.TidalDisplacement;
44 import org.orekit.orbits.KeplerianOrbit;
45 import org.orekit.orbits.Orbit;
46 import org.orekit.orbits.PositionAngleType;
47 import org.orekit.propagation.Propagator;
48 import org.orekit.propagation.conversion.PropagatorBuilder;
49 import org.orekit.time.AbsoluteDate;
50 import org.orekit.time.TimeScalesFactory;
51 import org.orekit.utils.Constants;
52 import org.orekit.utils.IERSConventions;
53 import org.orekit.utils.ParameterDriver;
54
55 import java.util.Arrays;
56 import java.util.HashMap;
57 import java.util.List;
58 import java.util.Map;
59
60 public class EcksteinHechlerEstimationTestUtils {
61
62 public static EcksteinHechlerContext smallEccentricContext(final String dataRoot) {
63
64 Utils.setDataRoot(dataRoot);
65 EcksteinHechlerContext context = new EcksteinHechlerContext();
66 context.conventions = IERSConventions.IERS_2010;
67 context.earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
68 Constants.WGS84_EARTH_FLATTENING,
69 FramesFactory.getITRF(context.conventions, true));
70 context.sun = CelestialBodyFactory.getSun();
71 context.moon = CelestialBodyFactory.getMoon();
72 final EOPHistory eopHistory = FramesFactory.getEOPHistory(context.conventions, true);
73 context.utc = TimeScalesFactory.getUTC();
74 context.ut1 = TimeScalesFactory.getUT1(eopHistory);
75 context.displacements = new StationDisplacement[] {
76 new TidalDisplacement(Constants.EIGEN5C_EARTH_EQUATORIAL_RADIUS,
77 Constants.JPL_SSD_SUN_EARTH_PLUS_MOON_MASS_RATIO,
78 Constants.JPL_SSD_EARTH_MOON_MASS_RATIO,
79 context.sun, context.moon,
80 context.conventions, false)
81 };
82 GravityFieldFactory.addPotentialCoefficientsReader(new GRGSFormatReader("grim4s4_gr", true));
83 AstronomicalAmplitudeReader aaReader =
84 new AstronomicalAmplitudeReader("hf-fes2004.dat", 5, 2, 3, 1.0);
85 DataContext.getDefault().getDataProvidersManager().feed(aaReader.getSupportedNames(), aaReader);
86 Map<Integer, Double> map = aaReader.getAstronomicalAmplitudesMap();
87 GravityFieldFactory.addOceanTidesReader(new FESCHatEpsilonReader("fes2004-7x7.dat",
88 0.01, FastMath.toRadians(1.0),
89 OceanLoadDeformationCoefficients.IERS_2010,
90 map));
91 context.gravity = GravityFieldFactory.getUnnormalizedProvider(6, 0);
92 context.initialOrbit = new KeplerianOrbit(15000000.0, 0.001, 1.25,
93 0.250, 1.375, 0.0625, PositionAngleType.TRUE,
94 FramesFactory.getEME2000(),
95 new AbsoluteDate(2000, 2, 24, 11, 35, 47.0, context.utc),
96 context.gravity.getMu());
97
98 context.stations = Arrays.asList(
99 context.createStation(-53.05388, -75.01551, 1750.0, "Isla Desolación"),
100 context.createStation( 62.29639, -7.01250, 880.0, "Slættaratindur")
101
102 );
103
104
105
106
107 context.TARstations = new HashMap<GroundStation, GroundStation>();
108
109 context.TARstations.put(context.createStation(-53.05388, -75.01551, 1750.0, "Isla Desolación"),
110 context.createStation(-54.815833, -68.317778, 6.0, "Ushuaïa"));
111
112 context.TARstations.put(context.createStation( 62.29639, -7.01250, 880.0, "Slættaratindur"),
113 context.createStation( 61.405833, -6.705278, 470.0, "Sumba"));
114
115 return context;
116
117 }
118
119 public static Propagator createPropagator(final Orbit initialOrbit,
120 final PropagatorBuilder propagatorBuilder) {
121
122
123 double[] orbitArray = new double[6];
124 propagatorBuilder.getOrbitType().mapOrbitToArray(initialOrbit,
125 propagatorBuilder.getPositionAngleType(),
126 orbitArray, null);
127 for (int i = 0; i < orbitArray.length; ++i) {
128
129 propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(i).setValue(orbitArray[i], initialOrbit.getDate());
130 }
131
132 return propagatorBuilder.buildPropagator();
133
134 }
135
136 public static List<ObservedMeasurement<?>> createMeasurements(final Propagator propagator,
137 final MeasurementCreator creator,
138 final double startPeriod, final double endPeriod,
139 final double step) {
140
141 propagator.setStepHandler(step, creator);
142 final double period = propagator.getInitialState().getOrbit().getKeplerianPeriod();
143 final AbsoluteDate start = propagator.getInitialState().getDate().shiftedBy(startPeriod * period);
144 final AbsoluteDate end = propagator.getInitialState().getDate().shiftedBy(endPeriod * period);
145 propagator.propagate(start, end);
146
147 final List<ObservedMeasurement<?>> measurements = creator.getMeasurements();
148
149 for (final ObservedMeasurement<?> measurement : measurements) {
150 for (final ParameterDriver driver : measurement.getParametersDrivers()) {
151 if (driver.getReferenceDate() == null) {
152 driver.setReferenceDate(propagator.getInitialState().getDate());
153 }
154 }
155 }
156
157 return measurements;
158
159 }
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176 public static void checkFit(final EcksteinHechlerContext context, final BatchLSEstimator estimator,
177 final int iterations, final int evaluations,
178 final double expectedRMS, final double rmsEps,
179 final double expectedMax, final double maxEps,
180 final double expectedDeltaPos, final double posEps,
181 final double expectedDeltaVel, final double velEps) {
182
183 final Orbit estimatedOrbit = estimator.estimate()[0].getInitialState().getOrbit();
184 final Vector3D estimatedPosition = estimatedOrbit.getPosition();
185 final Vector3D estimatedVelocity = estimatedOrbit.getVelocity();
186
187 Assertions.assertEquals(iterations, estimator.getIterationsCount());
188 Assertions.assertEquals(evaluations, estimator.getEvaluationsCount());
189 Optimum optimum = estimator.getOptimum();
190 Assertions.assertEquals(iterations, optimum.getIterations());
191 Assertions.assertEquals(evaluations, optimum.getEvaluations());
192
193 int k = 0;
194 double sum = 0;
195 double max = 0;
196 for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry :
197 estimator.getLastEstimations().entrySet()) {
198 final ObservedMeasurement<?> m = entry.getKey();
199 final EstimatedMeasurement<?> e = entry.getValue();
200 final double[] weight = m.getBaseWeight();
201 final double[] sigma = m.getTheoreticalStandardDeviation();
202 final double[] observed = m.getObservedValue();
203 final double[] theoretical = e.getEstimatedValue();
204 for (int i = 0; i < m.getDimension(); ++i) {
205 final double weightedResidual = weight[i] * (theoretical[i] - observed[i]) / sigma[i];
206 ++k;
207 sum += weightedResidual * weightedResidual;
208 max = FastMath.max(max, FastMath.abs(weightedResidual));
209 }
210 }
211
212 final double rms = FastMath.sqrt(sum / k);
213 final double deltaPos = Vector3D.distance(context.initialOrbit.getPosition(), estimatedPosition);
214 final double deltaVel = Vector3D.distance(context.initialOrbit.getVelocity(), estimatedVelocity);
215 Assertions.assertEquals(expectedRMS,
216 rms,
217 rmsEps);
218 Assertions.assertEquals(expectedMax,
219 max,
220 maxEps);
221 Assertions.assertEquals(expectedDeltaPos,
222 deltaPos,
223 posEps);
224 Assertions.assertEquals(expectedDeltaVel,
225 deltaVel,
226 velEps);
227
228 }
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245 public static void checkKalmanFit(final EcksteinHechlerContext context, final KalmanEstimator kalman,
246 final List<ObservedMeasurement<?>> measurements,
247 final Orbit refOrbit, final PositionAngleType positionAngleType,
248 final double expectedDeltaPos, final double posEps,
249 final double expectedDeltaVel, final double velEps,
250 final double[] expectedSigmasPos,final double sigmaPosEps,
251 final double[] expectedSigmasVel,final double sigmaVelEps)
252 {
253 checkKalmanFit(context, kalman, measurements,
254 new Orbit[] { refOrbit },
255 new PositionAngleType[] {positionAngleType},
256 new double[] { expectedDeltaPos }, new double[] { posEps },
257 new double[] { expectedDeltaVel }, new double[] { velEps },
258 new double[][] { expectedSigmasPos }, new double[] { sigmaPosEps },
259 new double[][] { expectedSigmasVel }, new double[] { sigmaVelEps });
260 }
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277 public static void checkKalmanFit(final EcksteinHechlerContext context, final KalmanEstimator kalman,
278 final List<ObservedMeasurement<?>> measurements,
279 final Orbit[] refOrbit, final PositionAngleType[] positionAngleType,
280 final double[] expectedDeltaPos, final double[] posEps,
281 final double[] expectedDeltaVel, final double []velEps,
282 final double[][] expectedSigmasPos,final double[] sigmaPosEps,
283 final double[][] expectedSigmasVel,final double[] sigmaVelEps)
284 {
285
286
287 Propagator[] estimated = kalman.processMeasurements(measurements);
288
289
290 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
291
292 for (int k = 0; k < refOrbit.length; ++k) {
293
294 final Orbit estimatedOrbit = estimated[k].getInitialState().getOrbit();
295 final Vector3D estimatedPosition = estimatedOrbit.getPosition();
296 final Vector3D estimatedVelocity = estimatedOrbit.getVelocity();
297
298
299 final RealMatrix estimatedP = kalman.getPhysicalEstimatedCovarianceMatrix();
300
301
302
303 final double[][] dCdY = new double[6][6];
304 estimatedOrbit.getJacobianWrtParameters(positionAngleType[k], dCdY);
305 final RealMatrix Jacobian = MatrixUtils.createRealMatrix(dCdY);
306 final RealMatrix estimatedCartesianP =
307 Jacobian.
308 multiply(estimatedP.getSubMatrix(0, 5, 0, 5)).
309 multiply(Jacobian.transpose());
310
311
312 final double[] sigmas = new double[6];
313 for (int i = 0; i < 6; i++) {
314 sigmas[i] = FastMath.sqrt(estimatedCartesianP.getEntry(i, i));
315 }
316
317
318 final double deltaPosK = Vector3D.distance(refOrbit[k].getPosition(), estimatedPosition);
319 final double deltaVelK = Vector3D.distance(refOrbit[k].getVelocity(), estimatedVelocity);
320 Assertions.assertEquals(expectedDeltaPos[k], deltaPosK, posEps[k]);
321 Assertions.assertEquals(expectedDeltaVel[k], deltaVelK, velEps[k]);
322
323 for (int i = 0; i < 3; i++) {
324 System.out.println(sigmas[i]);
325 System.out.println(sigmas[i+3]);
326 Assertions.assertEquals(expectedSigmasPos[k][i], sigmas[i], sigmaPosEps[k]);
327 Assertions.assertEquals(expectedSigmasVel[k][i], sigmas[i+3], sigmaVelEps[k]);
328 }
329 }
330 }
331
332 }