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.Assert;
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.GroundStation;
37 import org.orekit.estimation.measurements.MeasurementCreator;
38 import org.orekit.estimation.measurements.ObservedMeasurement;
39 import org.orekit.estimation.sequential.KalmanEstimator;
40 import org.orekit.forces.gravity.potential.AstronomicalAmplitudeReader;
41 import org.orekit.forces.gravity.potential.FESCHatEpsilonReader;
42 import org.orekit.forces.gravity.potential.GRGSFormatReader;
43 import org.orekit.forces.gravity.potential.GravityFieldFactory;
44 import org.orekit.forces.gravity.potential.OceanLoadDeformationCoefficients;
45 import org.orekit.frames.EOPHistory;
46 import org.orekit.frames.FramesFactory;
47 import org.orekit.models.earth.displacement.StationDisplacement;
48 import org.orekit.models.earth.displacement.TidalDisplacement;
49 import org.orekit.orbits.KeplerianOrbit;
50 import org.orekit.orbits.Orbit;
51 import org.orekit.orbits.PositionAngle;
52 import org.orekit.propagation.Propagator;
53 import org.orekit.propagation.conversion.PropagatorBuilder;
54 import org.orekit.time.AbsoluteDate;
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
61 public class BrouwerLyddaneEstimationTestUtils {
62
63 public static BrouwerLyddaneContext eccentricContext(final String dataRoot) {
64
65 Utils.setDataRoot(dataRoot);
66 BrouwerLyddaneContext context = new BrouwerLyddaneContext();
67 context.conventions = IERSConventions.IERS_2010;
68 context.earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
69 Constants.WGS84_EARTH_FLATTENING,
70 FramesFactory.getITRF(context.conventions, true));
71 context.sun = CelestialBodyFactory.getSun();
72 context.moon = CelestialBodyFactory.getMoon();
73 final EOPHistory eopHistory = FramesFactory.getEOPHistory(context.conventions, true);
74 context.utc = TimeScalesFactory.getUTC();
75 context.ut1 = TimeScalesFactory.getUT1(eopHistory);
76 context.displacements = new StationDisplacement[] {
77 new TidalDisplacement(Constants.EIGEN5C_EARTH_EQUATORIAL_RADIUS,
78 Constants.JPL_SSD_SUN_EARTH_PLUS_MOON_MASS_RATIO,
79 Constants.JPL_SSD_EARTH_MOON_MASS_RATIO,
80 context.sun, context.moon,
81 context.conventions, false)
82 };
83 GravityFieldFactory.addPotentialCoefficientsReader(new GRGSFormatReader("grim4s4_gr", true));
84 AstronomicalAmplitudeReader aaReader =
85 new AstronomicalAmplitudeReader("hf-fes2004.dat", 5, 2, 3, 1.0);
86 DataContext.getDefault().getDataProvidersManager().feed(aaReader.getSupportedNames(), aaReader);
87 Map<Integer, Double> map = aaReader.getAstronomicalAmplitudesMap();
88 GravityFieldFactory.addOceanTidesReader(new FESCHatEpsilonReader("fes2004-7x7.dat",
89 0.01, FastMath.toRadians(1.0),
90 OceanLoadDeformationCoefficients.IERS_2010,
91 map));
92 context.gravity = GravityFieldFactory.getUnnormalizedProvider(5, 0);
93 context.initialOrbit = new KeplerianOrbit(15000000.0, 0.125, 1.25,
94 0.250, 1.375, 0.0625, PositionAngle.TRUE,
95 FramesFactory.getEME2000(),
96 new AbsoluteDate(2000, 2, 24, 11, 35, 47.0, context.utc),
97 context.gravity.getMu());
98
99 context.stations = Arrays.asList(
100 context.createStation(-53.05388, -75.01551, 1750.0, "Isla Desolación"),
101 context.createStation( 62.29639, -7.01250, 880.0, "Slættaratindur")
102
103 );
104
105
106
107
108 context.TARstations = new HashMap<GroundStation, GroundStation>();
109
110 context.TARstations.put(context.createStation(-53.05388, -75.01551, 1750.0, "Isla Desolación"),
111 context.createStation(-54.815833, -68.317778, 6.0, "Ushuaïa"));
112
113 context.TARstations.put(context.createStation( 62.29639, -7.01250, 880.0, "Slættaratindur"),
114 context.createStation( 61.405833, -6.705278, 470.0, "Sumba"));
115
116 return context;
117
118 }
119
120 public static Propagator createPropagator(final Orbit initialOrbit,
121 final PropagatorBuilder propagatorBuilder) {
122
123
124 double[] orbitArray = new double[6];
125 propagatorBuilder.getOrbitType().mapOrbitToArray(initialOrbit,
126 propagatorBuilder.getPositionAngle(),
127 orbitArray, null);
128 for (int i = 0; i < orbitArray.length; ++i) {
129 propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(i).setValue(orbitArray[i]);
130 }
131
132 return propagatorBuilder.buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
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().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 BrouwerLyddaneContext 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.getPVCoordinates().getPosition();
185 final Vector3D estimatedVelocity = estimatedOrbit.getPVCoordinates().getVelocity();
186
187 Assert.assertEquals(iterations, estimator.getIterationsCount());
188 Assert.assertEquals(evaluations, estimator.getEvaluationsCount());
189 Optimum optimum = estimator.getOptimum();
190 Assert.assertEquals(iterations, optimum.getIterations());
191 Assert.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.getPVCoordinates().getPosition(), estimatedPosition);
214 final double deltaVel = Vector3D.distance(context.initialOrbit.getPVCoordinates().getVelocity(), estimatedVelocity);
215 Assert.assertEquals(expectedRMS,
216 rms,
217 rmsEps);
218 Assert.assertEquals(expectedMax,
219 max,
220 maxEps);
221 Assert.assertEquals(expectedDeltaPos,
222 deltaPos,
223 posEps);
224 Assert.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 BrouwerLyddaneContext context, final KalmanEstimator kalman,
246 final List<ObservedMeasurement<?>> measurements,
247 final Orbit refOrbit, final PositionAngle positionAngle,
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 PositionAngle[] { positionAngle },
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 BrouwerLyddaneContext context, final KalmanEstimator kalman,
278 final List<ObservedMeasurement<?>> measurements,
279 final Orbit[] refOrbit, final PositionAngle[] positionAngle,
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 Assert.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.getPVCoordinates().getPosition();
296 final Vector3D estimatedVelocity = estimatedOrbit.getPVCoordinates().getVelocity();
297
298
299 final RealMatrix estimatedP = kalman.getPhysicalEstimatedCovarianceMatrix();
300
301
302
303 final double[][] dCdY = new double[6][6];
304 estimatedOrbit.getJacobianWrtParameters(positionAngle[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].getPVCoordinates().getPosition(), estimatedPosition);
319 final double deltaVelK = Vector3D.distance(refOrbit[k].getPVCoordinates().getVelocity(), estimatedVelocity);
320 Assert.assertEquals(expectedDeltaPos[k], deltaPosK, posEps[k]);
321 Assert.assertEquals(expectedDeltaVel[k], deltaVelK, velEps[k]);
322
323 for (int i = 0; i < 3; i++) {
324 Assert.assertEquals(expectedSigmasPos[k][i], sigmas[i], sigmaPosEps[k]);
325 Assert.assertEquals(expectedSigmasVel[k][i], sigmas[i+3], sigmaVelEps[k]);
326 }
327 }
328 }
329
330 }
331