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