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.HashMap;
21 import java.util.List;
22 import java.util.Map;
23
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.data.DataContext;
34 import org.orekit.estimation.leastsquares.BatchLSEstimator;
35 import org.orekit.estimation.measurements.EstimatedMeasurement;
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.forces.gravity.potential.AstronomicalAmplitudeReader;
40 import org.orekit.forces.gravity.potential.FESCHatEpsilonReader;
41 import org.orekit.forces.gravity.potential.GRGSFormatReader;
42 import org.orekit.forces.gravity.potential.GravityFieldFactory;
43 import org.orekit.forces.gravity.potential.OceanLoadDeformationCoefficients;
44 import org.orekit.frames.EOPHistory;
45 import org.orekit.frames.FramesFactory;
46 import org.orekit.models.earth.displacement.StationDisplacement;
47 import org.orekit.models.earth.displacement.TidalDisplacement;
48 import org.orekit.orbits.KeplerianOrbit;
49 import org.orekit.orbits.Orbit;
50 import org.orekit.orbits.PositionAngleType;
51 import org.orekit.propagation.Propagator;
52 import org.orekit.propagation.conversion.PropagatorBuilder;
53 import org.orekit.time.AbsoluteDate;
54 import org.orekit.time.TimeScalesFactory;
55 import org.orekit.utils.Constants;
56 import org.orekit.utils.IERSConventions;
57 import org.orekit.utils.ParameterDriver;
58
59
60 public class BrouwerLyddaneEstimationTestUtils {
61
62 public static BrouwerLyddaneContext eccentricContext(final String dataRoot) {
63
64 Utils.setDataRoot(dataRoot);
65 BrouwerLyddaneContext context = new BrouwerLyddaneContext();
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(5, 0);
92 context.initialOrbit = new KeplerianOrbit(15000000.0, 0.125, 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<>();
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 propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(i).setValue(orbitArray[i], initialOrbit.getDate());
129 }
130
131 return propagatorBuilder.buildPropagator();
132
133 }
134
135 public static List<ObservedMeasurement<?>> createMeasurements(final Propagator propagator,
136 final MeasurementCreator creator,
137 final double startPeriod, final double endPeriod,
138 final double step) {
139
140 propagator.setStepHandler(step, creator);
141 final double period = propagator.getInitialState().getOrbit().getKeplerianPeriod();
142 final AbsoluteDate start = propagator.getInitialState().getDate().shiftedBy(startPeriod * period);
143 final AbsoluteDate end = propagator.getInitialState().getDate().shiftedBy(endPeriod * period);
144 propagator.propagate(start, end);
145
146 final List<ObservedMeasurement<?>> measurements = creator.getMeasurements();
147
148 for (final ObservedMeasurement<?> measurement : measurements) {
149 for (final ParameterDriver driver : measurement.getParametersDrivers()) {
150 if (driver.getReferenceDate() == null) {
151 driver.setReferenceDate(propagator.getInitialState().getDate());
152 }
153 }
154 }
155
156 return measurements;
157
158 }
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175 public static void checkFit(final BrouwerLyddaneContext context, final BatchLSEstimator estimator,
176 final int iterations, final int evaluations,
177 final double expectedRMS, final double rmsEps,
178 final double expectedMax, final double maxEps,
179 final double expectedDeltaPos, final double posEps,
180 final double expectedDeltaVel, final double velEps) {
181
182 final Orbit estimatedOrbit = estimator.estimate()[0].getInitialState().getOrbit();
183 final Vector3D estimatedPosition = estimatedOrbit.getPosition();
184 final Vector3D estimatedVelocity = estimatedOrbit.getPVCoordinates().getVelocity();
185
186 Assertions.assertEquals(iterations, estimator.getIterationsCount());
187 Assertions.assertEquals(evaluations, estimator.getEvaluationsCount());
188 Optimum optimum = estimator.getOptimum();
189 Assertions.assertEquals(iterations, optimum.getIterations());
190 Assertions.assertEquals(evaluations, optimum.getEvaluations());
191
192 int k = 0;
193 double sum = 0;
194 double max = 0;
195 for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry :
196 estimator.getLastEstimations().entrySet()) {
197 final ObservedMeasurement<?> m = entry.getKey();
198 final EstimatedMeasurement<?> e = entry.getValue();
199 final double[] weight = m.getBaseWeight();
200 final double[] sigma = m.getTheoreticalStandardDeviation();
201 final double[] observed = m.getObservedValue();
202 final double[] theoretical = e.getEstimatedValue();
203 for (int i = 0; i < m.getDimension(); ++i) {
204 final double weightedResidual = weight[i] * (theoretical[i] - observed[i]) / sigma[i];
205 ++k;
206 sum += weightedResidual * weightedResidual;
207 max = FastMath.max(max, FastMath.abs(weightedResidual));
208 }
209 }
210
211 final double rms = FastMath.sqrt(sum / k);
212 final double deltaPos = Vector3D.distance(context.initialOrbit.getPosition(), estimatedPosition);
213 final double deltaVel = Vector3D.distance(context.initialOrbit.getPVCoordinates().getVelocity(), estimatedVelocity);
214 Assertions.assertEquals(expectedRMS,
215 rms,
216 rmsEps);
217 Assertions.assertEquals(expectedMax,
218 max,
219 maxEps);
220 Assertions.assertEquals(expectedDeltaPos,
221 deltaPos,
222 posEps);
223 Assertions.assertEquals(expectedDeltaVel,
224 deltaVel,
225 velEps);
226
227 }
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244 public static void checkKalmanFit(final BrouwerLyddaneContext context, final KalmanEstimator kalman,
245 final List<ObservedMeasurement<?>> measurements,
246 final Orbit refOrbit, final PositionAngleType positionAngleType,
247 final double expectedDeltaPos, final double posEps,
248 final double expectedDeltaVel, final double velEps,
249 final double[] expectedSigmasPos,final double sigmaPosEps,
250 final double[] expectedSigmasVel,final double sigmaVelEps)
251 {
252 checkKalmanFit(context, kalman, measurements,
253 new Orbit[] { refOrbit },
254 new PositionAngleType[] {positionAngleType},
255 new double[] { expectedDeltaPos }, new double[] { posEps },
256 new double[] { expectedDeltaVel }, new double[] { velEps },
257 new double[][] { expectedSigmasPos }, new double[] { sigmaPosEps },
258 new double[][] { expectedSigmasVel }, new double[] { sigmaVelEps });
259 }
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276 public static void checkKalmanFit(final BrouwerLyddaneContext context, final KalmanEstimator kalman,
277 final List<ObservedMeasurement<?>> measurements,
278 final Orbit[] refOrbit, final PositionAngleType[] positionAngleType,
279 final double[] expectedDeltaPos, final double[] posEps,
280 final double[] expectedDeltaVel, final double []velEps,
281 final double[][] expectedSigmasPos,final double[] sigmaPosEps,
282 final double[][] expectedSigmasVel,final double[] sigmaVelEps)
283 {
284
285
286 Propagator[] estimated = kalman.processMeasurements(measurements);
287
288
289 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
290
291 for (int k = 0; k < refOrbit.length; ++k) {
292
293 final Orbit estimatedOrbit = estimated[k].getInitialState().getOrbit();
294 final Vector3D estimatedPosition = estimatedOrbit.getPosition();
295 final Vector3D estimatedVelocity = estimatedOrbit.getPVCoordinates().getVelocity();
296
297
298 final RealMatrix estimatedP = kalman.getPhysicalEstimatedCovarianceMatrix();
299
300
301
302 final double[][] dCdY = new double[6][6];
303 estimatedOrbit.getJacobianWrtParameters(positionAngleType[k], dCdY);
304 final RealMatrix Jacobian = MatrixUtils.createRealMatrix(dCdY);
305 final RealMatrix estimatedCartesianP =
306 Jacobian.
307 multiply(estimatedP.getSubMatrix(0, 5, 0, 5)).
308 multiply(Jacobian.transpose());
309
310
311 final double[] sigmas = new double[6];
312 for (int i = 0; i < 6; i++) {
313 sigmas[i] = FastMath.sqrt(estimatedCartesianP.getEntry(i, i));
314 }
315
316
317 final double deltaPosK = Vector3D.distance(refOrbit[k].getPosition(), estimatedPosition);
318 final double deltaVelK = Vector3D.distance(refOrbit[k].getPVCoordinates().getVelocity(), estimatedVelocity);
319 Assertions.assertEquals(expectedDeltaPos[k], deltaPosK, posEps[k]);
320 Assertions.assertEquals(expectedDeltaVel[k], deltaVelK, velEps[k]);
321
322 for (int i = 0; i < 3; i++) {
323 Assertions.assertEquals(expectedSigmasPos[k][i], sigmas[i], sigmaPosEps[k]);
324 Assertions.assertEquals(expectedSigmasVel[k][i], sigmas[i+3], sigmaVelEps[k]);
325 }
326 }
327 }
328
329 }
330