1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.models.earth.troposphere;
18
19 import org.hipparchus.Field;
20 import org.hipparchus.analysis.differentiation.DSFactory;
21 import org.hipparchus.analysis.differentiation.DerivativeStructure;
22 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
23 import org.hipparchus.geometry.euclidean.threed.Vector3D;
24 import org.hipparchus.util.Binary64Field;
25 import org.hipparchus.util.FastMath;
26 import org.junit.jupiter.api.Assertions;
27 import org.junit.jupiter.api.Test;
28 import org.orekit.attitudes.Attitude;
29 import org.orekit.bodies.FieldGeodeticPoint;
30 import org.orekit.bodies.GeodeticPoint;
31 import org.orekit.bodies.OneAxisEllipsoid;
32 import org.orekit.estimation.measurements.GroundStation;
33 import org.orekit.frames.Frame;
34 import org.orekit.frames.FramesFactory;
35 import org.orekit.frames.TopocentricFrame;
36 import org.orekit.models.earth.weather.PressureTemperatureHumidityProvider;
37 import org.orekit.orbits.FieldKeplerianOrbit;
38 import org.orekit.orbits.FieldOrbit;
39 import org.orekit.orbits.Orbit;
40 import org.orekit.orbits.OrbitType;
41 import org.orekit.orbits.PositionAngleType;
42 import org.orekit.propagation.FieldSpacecraftState;
43 import org.orekit.propagation.SpacecraftState;
44 import org.orekit.propagation.ToleranceProvider;
45 import org.orekit.time.AbsoluteDate;
46 import org.orekit.time.FieldAbsoluteDate;
47 import org.orekit.time.TimeScalesFactory;
48 import org.orekit.utils.Constants;
49 import org.orekit.utils.FieldTrackingCoordinates;
50 import org.orekit.utils.IERSConventions;
51 import org.orekit.utils.ParameterDriver;
52 import org.orekit.utils.ParameterDriversList;
53 import org.orekit.utils.TrackingCoordinates;
54
55 import java.util.List;
56
57 public class EstimatedModelTest extends AbstractPathDelayTest<EstimatedModel> {
58
59 @Override
60 protected EstimatedModel buildTroposphericModel(final PressureTemperatureHumidityProvider provider) {
61 return new EstimatedModel(new NiellMappingFunctionModel(), 2.0);
62 }
63
64 @Test
65 @Override
66 public void testDelay() {
67 doTestDelay(defaultDate, defaultPoint, defaultTrackingCoordinates, null,
68 2.09133, -0.09133, 3.39014, -0.14822, 3.24193);
69 }
70
71 @Test
72 @Override
73 public void testFieldDelay() {
74 doTestDelay(Binary64Field.getInstance(),
75 defaultDate, defaultPoint, defaultTrackingCoordinates, null,
76 2.09133, -0.09133, 3.39014, -0.14822, 3.24193);
77 }
78
79 @Override
80 @Test
81 public void testFixedHeight() {
82 doTestFixedHeight(null);
83 }
84
85 @Override
86 @Test
87 public void testFieldFixedHeight() {
88 doTestFieldFixedHeight(Binary64Field.getInstance(), null);
89 }
90
91 @Override
92 @Test
93 public void testFixedElevation() {
94 doTestFixedElevation(null);
95 }
96
97 @Override
98 @Test
99 public void testFieldFixedElevation() {
100 doTestFieldFixedElevation(Binary64Field.getInstance(), null);
101 }
102
103 @Test
104 public void testStateDerivativesGMF() {
105 final double latitude = FastMath.toRadians(45.0);
106 final double longitude = FastMath.toRadians(45.0);
107 GeodeticPoint point = new GeodeticPoint(latitude, longitude, 0.0);
108 final TroposphereMappingFunction gmf = new GlobalMappingFunctionModel();
109 doTestDelayStateDerivatives(gmf, point, 4.7e-9);
110 }
111
112 @Test
113 public void testStateDerivativesNMF() {
114 final double latitude = FastMath.toRadians(45.0);
115 final double longitude = FastMath.toRadians(45.0);
116 GeodeticPoint point = new GeodeticPoint(latitude, longitude, 0.0);
117 final TroposphereMappingFunction nmf = new NiellMappingFunctionModel();
118 doTestDelayStateDerivatives(nmf, point, 4.4e-9);
119 }
120
121 private void doTestDelayStateDerivatives(final TroposphereMappingFunction func, final GeodeticPoint point, final double tolerance) {
122
123
124 final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
125 Constants.WGS84_EARTH_FLATTENING,
126 FramesFactory.getITRF(IERSConventions.IERS_2010, true));
127
128 final TopocentricFrame baseFrame = new TopocentricFrame(earth, point, "topo");
129
130
131 final GroundStation station = new GroundStation(baseFrame);
132
133
134 final TroposphericModel model = new EstimatedModel(func, 2.0);
135
136
137 final DSFactory factory = new DSFactory(6, 1);
138 final DerivativeStructure a0 = factory.variable(0, 24464560.0);
139 final DerivativeStructure e0 = factory.variable(1, 0.05);
140 final DerivativeStructure i0 = factory.variable(2, 0.122138);
141 final DerivativeStructure pa0 = factory.variable(3, 3.10686);
142 final DerivativeStructure raan0 = factory.variable(4, 1.00681);
143 final DerivativeStructure anomaly0 = factory.variable(5, 0.048363);
144 final Field<DerivativeStructure> field = a0.getField();
145 final DerivativeStructure zero = field.getZero();
146
147
148 final FieldAbsoluteDate<DerivativeStructure> dsDate = new FieldAbsoluteDate<>(field);
149
150 final Frame frame = FramesFactory.getEME2000();
151 final FieldOrbit<DerivativeStructure> dsOrbit = new FieldKeplerianOrbit<>(a0, e0, i0, pa0, raan0, anomaly0,
152 PositionAngleType.MEAN, frame,
153 dsDate, zero.add(3.9860047e14));
154
155 final FieldSpacecraftState<DerivativeStructure> dsState = new FieldSpacecraftState<>(dsOrbit);
156
157
158 final FieldVector3D<DerivativeStructure> position = dsState.getPosition();
159 final FieldTrackingCoordinates<DerivativeStructure> dsTrackingCoordinates =
160 baseFrame.getTrackingCoordinates(position, frame, dsDate);
161
162
163 for (final ParameterDriver driver : model.getParametersDrivers()) {
164 driver.setReferenceDate(dsDate.toAbsoluteDate());
165 }
166
167
168 final FieldGeodeticPoint<DerivativeStructure> dsPoint = new FieldGeodeticPoint<>(zero.add(point.getLatitude()), zero.add(point.getLongitude()), zero.add(point.getAltitude()));
169 final DerivativeStructure delay = model.pathDelay(dsTrackingCoordinates, dsPoint,
170 model.getParameters(field), dsDate).getDelay();
171
172 final double[] compDeriv = delay.getAllDerivatives();
173
174
175 final Orbit orbit = dsOrbit.toOrbit();
176 final SpacecraftState state = dsState.toSpacecraftState();
177
178
179 final double[][] refDeriv = new double[1][6];
180 final OrbitType orbitType = OrbitType.KEPLERIAN;
181 final PositionAngleType angleType = PositionAngleType.MEAN;
182 double dP = 0.001;
183 double[] steps = ToleranceProvider.getDefaultToleranceProvider(1000000 * dP).getTolerances(orbit, orbitType)[0];
184 for (int i = 0; i < 6; i++) {
185 SpacecraftState stateM4 = shiftState(state, orbitType, angleType, -4 * steps[i], i);
186 final Vector3D positionM4 = stateM4.getPosition();
187 final TrackingCoordinates trackingCoordinatesM4 = station.getBaseFrame().
188 getTrackingCoordinates(positionM4, stateM4.getFrame(), stateM4.getDate());
189 double delayM4 = model.pathDelay(trackingCoordinatesM4, point,
190 model.getParameters(), stateM4.getDate()).getDelay();
191
192 SpacecraftState stateM3 = shiftState(state, orbitType, angleType, -3 * steps[i], i);
193 final Vector3D positionM3 = stateM3.getPosition();
194 final TrackingCoordinates trackingCoordinatesM3 = station.getBaseFrame().
195 getTrackingCoordinates(positionM3, stateM3.getFrame(), stateM3.getDate());
196 double delayM3 = model.pathDelay(trackingCoordinatesM3, point,
197 model.getParameters(), stateM3.getDate()).getDelay();
198
199 SpacecraftState stateM2 = shiftState(state, orbitType, angleType, -2 * steps[i], i);
200 final Vector3D positionM2 = stateM2.getPosition();
201 final TrackingCoordinates trackingCoordinatesM2 = station.getBaseFrame().
202 getTrackingCoordinates(positionM2, stateM2.getFrame(), stateM2.getDate());
203 double delayM2 = model.pathDelay(trackingCoordinatesM2, point,
204 model.getParameters(), stateM2.getDate()).getDelay();
205
206 SpacecraftState stateM1 = shiftState(state, orbitType, angleType, -1 * steps[i], i);
207 final Vector3D positionM1 = stateM1.getPosition();
208 final TrackingCoordinates trackingCoordinatesM1 = station.getBaseFrame().
209 getTrackingCoordinates(positionM1, stateM1.getFrame(), stateM1.getDate());
210 double delayM1 = model.pathDelay(trackingCoordinatesM1, point,
211 model.getParameters(), stateM1.getDate()).getDelay();
212
213 SpacecraftState stateP1 = shiftState(state, orbitType, angleType, 1 * steps[i], i);
214 final Vector3D positionP1 = stateP1.getPosition();
215 final TrackingCoordinates trackingCoordinatesP1 = station.getBaseFrame().
216 getTrackingCoordinates(positionP1, stateP1.getFrame(), stateP1.getDate());
217 double delayP1 = model.pathDelay(trackingCoordinatesP1, point,
218 model.getParameters(), stateP1.getDate()).getDelay();
219
220 SpacecraftState stateP2 = shiftState(state, orbitType, angleType, 2 * steps[i], i);
221 final Vector3D positionP2 = stateP2.getPosition();
222 final TrackingCoordinates trackingCoordinatesP2 = station.getBaseFrame().
223 getTrackingCoordinates(positionP2, stateP2.getFrame(), stateP2.getDate());
224 double delayP2 = model.pathDelay(trackingCoordinatesP2, point,
225 model.getParameters(), stateP2.getDate()).getDelay();
226
227 SpacecraftState stateP3 = shiftState(state, orbitType, angleType, 3 * steps[i], i);
228 final Vector3D positionP3 = stateP3.getPosition();
229 final TrackingCoordinates trackingCoordinatesP3 = station.getBaseFrame().
230 getTrackingCoordinates(positionP3, stateP3.getFrame(), stateP3.getDate());
231 double delayP3 = model.pathDelay(trackingCoordinatesP3, point,
232 model.getParameters(), stateP3.getDate()).getDelay();
233
234 SpacecraftState stateP4 = shiftState(state, orbitType, angleType, 4 * steps[i], i);
235 final Vector3D positionP4 = stateP4.getPosition();
236 final TrackingCoordinates trackingCoordinatesP4 = station.getBaseFrame().
237 getTrackingCoordinates(positionP4, stateP4.getFrame(), stateP4.getDate());
238 double delayP4 = model.pathDelay(trackingCoordinatesP4, point,
239 model.getParameters(), stateP4.getDate()).getDelay();
240
241 fillJacobianColumn(refDeriv, i, steps[i],
242 delayM4, delayM3, delayM2, delayM1,
243 delayP1, delayP2, delayP3, delayP4);
244 }
245
246 for (int i = 0; i < 6; i++) {
247 Assertions.assertEquals(compDeriv[i + 1], refDeriv[0][i], tolerance);
248 }
249 }
250
251 @Test
252 public void testDelayParameterDerivative() {
253 doTestParametersDerivatives(EstimatedModel.TOTAL_ZENITH_DELAY, 5.0e-15);
254 }
255
256 private void doTestParametersDerivatives(String parameterName, double tolerance) {
257
258
259 final double latitude = FastMath.toRadians(45.0);
260 final double longitude = FastMath.toRadians(45.0);
261 final double height = 0.0;
262 final GeodeticPoint point = new GeodeticPoint(latitude, longitude, height);
263
264 final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
265 Constants.WGS84_EARTH_FLATTENING,
266 FramesFactory.getITRF(IERSConventions.IERS_2010, true));
267
268 final TopocentricFrame baseFrame = new TopocentricFrame(earth, point, "topo");
269
270
271 final TroposphereMappingFunction gmf = new GlobalMappingFunctionModel();
272 final TroposphericModel model = new EstimatedModel(gmf, 5.0);
273
274
275 for (final ParameterDriver driver : model.getParametersDrivers()) {
276 driver.setValue(driver.getReferenceValue());
277 driver.setSelected(driver.getName().equals(parameterName));
278 }
279
280
281 int nbParams = 0;
282 for (final ParameterDriver driver : model.getParametersDrivers()) {
283 if (driver.isSelected()) {
284 ++nbParams;
285 }
286 }
287
288
289 final DSFactory factory = new DSFactory(6 + nbParams, 1);
290 final DerivativeStructure a0 = factory.variable(0, 24464560.0);
291 final DerivativeStructure e0 = factory.variable(1, 0.05);
292 final DerivativeStructure i0 = factory.variable(2, 0.122138);
293 final DerivativeStructure pa0 = factory.variable(3, 3.10686);
294 final DerivativeStructure raan0 = factory.variable(4, 1.00681);
295 final DerivativeStructure anomaly0 = factory.variable(5, 0.048363);
296 final Field<DerivativeStructure> field = a0.getField();
297 final DerivativeStructure zero = field.getZero();
298
299
300 final FieldAbsoluteDate<DerivativeStructure> dsDate = new FieldAbsoluteDate<>(field, 2018, 11, 19, 18, 0, 0.0,
301 TimeScalesFactory.getUTC());
302
303
304 for (final ParameterDriver driver : model.getParametersDrivers()) {
305 driver.setReferenceDate(dsDate.toAbsoluteDate());
306 }
307
308
309 final Frame frame = FramesFactory.getEME2000();
310 final FieldOrbit<DerivativeStructure> dsOrbit = new FieldKeplerianOrbit<>(a0, e0, i0, pa0, raan0, anomaly0,
311 PositionAngleType.MEAN, frame,
312 dsDate, zero.add(3.9860047e14));
313
314
315 final FieldSpacecraftState<DerivativeStructure> dsState = new FieldSpacecraftState<>(dsOrbit);
316
317
318 final FieldVector3D<DerivativeStructure> position = dsState.getPosition();
319 final FieldTrackingCoordinates<DerivativeStructure> dsTrackingCoordinates =
320 baseFrame.getTrackingCoordinates(position, frame, dsState.getDate());
321
322
323 final List<ParameterDriver> drivers = model.getParametersDrivers();
324 final DerivativeStructure[] parameters = new DerivativeStructure[drivers.size()];
325 int index = 6;
326 for (int i = 0; i < drivers.size(); ++i) {
327 parameters[i] = drivers.get(i).isSelected() ?
328 factory.variable(index++, drivers.get(i).getValue()) :
329 factory.constant(drivers.get(i).getValue());
330 }
331
332
333 final FieldGeodeticPoint<DerivativeStructure> dsPoint = new FieldGeodeticPoint<>(zero.add(point.getLatitude()), zero.add(point.getLongitude()), zero.add(point.getAltitude()));
334 final DerivativeStructure delay = model.pathDelay(dsTrackingCoordinates, dsPoint,
335 parameters, dsState.getDate()).getDelay();
336
337 final double[] compDeriv = delay.getAllDerivatives();
338
339
340 final SpacecraftState state = dsState.toSpacecraftState();
341 final TrackingCoordinates trackingCoordinates = new TrackingCoordinates(dsTrackingCoordinates.getAzimuth().getReal(),
342 dsTrackingCoordinates.getElevation().getReal(),
343 dsTrackingCoordinates.getRange().getReal());
344
345
346 final double[][] refDeriv = new double[1][1];
347 ParameterDriversList bound = new ParameterDriversList();
348 for (final ParameterDriver driver : model.getParametersDrivers()) {
349 if (driver.getName().equals(parameterName)) {
350 driver.setSelected(true);
351 bound.add(driver);
352 } else {
353 driver.setSelected(false);
354 }
355 }
356 ParameterDriver selected = bound.getDrivers().get(0);
357 double p0 = selected.getReferenceValue();
358 double h = selected.getScale();
359
360 final OrbitType orbitType = OrbitType.KEPLERIAN;
361 final PositionAngleType angleType = PositionAngleType.MEAN;
362
363 selected.setValue(p0 - 4 * h);
364 double delayM4 = model.pathDelay(trackingCoordinates, point,
365 model.getParameters(), state.getDate()).getDelay();
366
367 selected.setValue(p0 - 3 * h);
368 double delayM3 = model.pathDelay(trackingCoordinates, point,
369 model.getParameters(), state.getDate()).getDelay();
370
371 selected.setValue(p0 - 2 * h);
372 double delayM2 = model.pathDelay(trackingCoordinates, point,
373 model.getParameters(), state.getDate()).getDelay();
374
375 selected.setValue(p0 - 1 * h);
376 double delayM1 = model.pathDelay(trackingCoordinates, point,
377 model.getParameters(), state.getDate()).getDelay();
378
379 selected.setValue(p0 + 1 * h);
380 double delayP1 = model.pathDelay(trackingCoordinates, point,
381 model.getParameters(), state.getDate()).getDelay();
382
383 selected.setValue(p0 + 2 * h);
384 double delayP2 = model.pathDelay(trackingCoordinates, point,
385 model.getParameters(), state.getDate()).getDelay();
386
387 selected.setValue(p0 + 3 * h);
388 double delayP3 = model.pathDelay(trackingCoordinates, point,
389 model.getParameters(), state.getDate()).getDelay();
390
391 selected.setValue(p0 + 4 * h);
392 double delayP4 = model.pathDelay(trackingCoordinates, point,
393 model.getParameters(), state.getDate()).getDelay();
394
395 fillJacobianColumn(refDeriv, 0, h,
396 delayM4, delayM3, delayM2, delayM1,
397 delayP1, delayP2, delayP3, delayP4);
398
399 Assertions.assertEquals(compDeriv[7], refDeriv[0][0], tolerance);
400
401 }
402
403 private SpacecraftState shiftState(SpacecraftState state, OrbitType orbitType, PositionAngleType angleType,
404 double delta, int column) {
405
406 double[][] array = stateToArray(state, orbitType, angleType, true);
407 array[0][column] += delta;
408
409 return arrayToState(array, orbitType, angleType, state.getFrame(), state.getDate(),
410 state.getOrbit().getMu(), state.getAttitude());
411
412 }
413
414 private double[][] stateToArray(SpacecraftState state, OrbitType orbitType, PositionAngleType angleType,
415 boolean withMass) {
416 double[][] array = new double[2][withMass ? 7 : 6];
417 orbitType.mapOrbitToArray(state.getOrbit(), angleType, array[0], array[1]);
418 if (withMass) {
419 array[0][6] = state.getMass();
420 }
421 return array;
422 }
423
424 private SpacecraftState arrayToState(double[][] array, OrbitType orbitType, PositionAngleType angleType,
425 Frame frame, AbsoluteDate date, double mu, Attitude attitude) {
426 Orbit orbit = orbitType.mapArrayToOrbit(array[0], array[1], angleType, date, mu, frame);
427 return (array.length > 6) ?
428 new SpacecraftState(orbit, attitude) :
429 new SpacecraftState(orbit, attitude).withMass(array[0][6]);
430 }
431
432 private void fillJacobianColumn(double[][] jacobian, int column, double h,
433 double sM4h, double sM3h,
434 double sM2h, double sM1h,
435 double sP1h, double sP2h,
436 double sP3h, double sP4h) {
437
438 jacobian[0][column] = ( -3 * (sP4h - sM4h) +
439 32 * (sP3h - sM3h) -
440 168 * (sP2h - sM2h) +
441 672 * (sP1h - sM1h)) / (840 * h);
442 }
443
444 }