1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.propagation.analytical.gnss;
18
19 import java.util.ArrayList;
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.util.FastMath;
26 import org.hipparchus.util.Precision;
27 import org.junit.Assert;
28 import org.junit.BeforeClass;
29 import org.junit.Test;
30 import org.orekit.Utils;
31 import org.orekit.errors.OrekitException;
32 import org.orekit.errors.OrekitMessages;
33 import org.orekit.frames.Frame;
34 import org.orekit.frames.FramesFactory;
35 import org.orekit.gnss.SEMParser;
36 import org.orekit.gnss.SatelliteSystem;
37 import org.orekit.propagation.SpacecraftState;
38 import org.orekit.propagation.analytical.gnss.data.GNSSOrbitalElements;
39 import org.orekit.propagation.analytical.gnss.data.GPSAlmanac;
40 import org.orekit.propagation.analytical.gnss.data.GPSNavigationMessage;
41 import org.orekit.propagation.analytical.tle.TLE;
42 import org.orekit.propagation.analytical.tle.TLEPropagator;
43 import org.orekit.time.AbsoluteDate;
44 import org.orekit.time.DateComponents;
45 import org.orekit.time.GNSSDate;
46 import org.orekit.time.TimeScalesFactory;
47 import org.orekit.utils.CartesianDerivativesFilter;
48 import org.orekit.utils.Constants;
49 import org.orekit.utils.IERSConventions;
50 import org.orekit.utils.PVCoordinates;
51 import org.orekit.utils.TimeStampedPVCoordinates;
52
53
54 public class GPSPropagatorTest {
55
56 private static List<GPSAlmanac> almanacs;
57
58 @BeforeClass
59 public static void setUpBeforeClass() {
60 Utils.setDataRoot("gnss");
61 GNSSDate.setRolloverReference(new DateComponents(DateComponents.GPS_EPOCH, 7 * 512));
62
63 SEMParser reader = new SEMParser(null);
64
65 reader.loadData();
66
67 almanacs = reader.getAlmanacs();
68 }
69
70 @Test
71 public void testClockCorrections() {
72 final GNSSPropagator propagator = new GNSSPropagatorBuilder(almanacs.get(0)).build();
73 propagator.addAdditionalStateProvider(new ClockCorrectionsProvider(almanacs.get(0)));
74
75 final AbsoluteDate date0 = almanacs.get(0).getDate();
76 double dtRelMin = 0;
77 double dtRelMax = 0;
78 for (double dt = 0; dt < 0.5 * Constants.JULIAN_DAY; dt += 1.0) {
79 SpacecraftState state = propagator.propagate(date0.shiftedBy(dt));
80 double[] corrections = state.getAdditionalState(ClockCorrectionsProvider.CLOCK_CORRECTIONS);
81 Assert.assertEquals(3, corrections.length);
82 Assert.assertEquals(1.33514404296875E-05, corrections[0], 1.0e-19);
83 dtRelMin = FastMath.min(dtRelMin, corrections[1]);
84 dtRelMax = FastMath.max(dtRelMax, corrections[1]);
85 Assert.assertEquals(0.0, corrections[2], Precision.SAFE_MIN);
86 }
87 Assert.assertEquals(0.0, almanacs.get(0).getToc(), 1.0e-12);
88 Assert.assertEquals(-1.1679e-8, dtRelMin, 1.0e-12);
89 Assert.assertEquals(+1.1679e-8, dtRelMax, 1.0e-12);
90 }
91
92 @Test
93 public void testGPSCycle() {
94
95 final GNSSPropagator propagator = new GNSSPropagatorBuilder(almanacs.get(0)).
96 attitudeProvider(Utils.defaultLaw()).
97 mass(1521.0).
98 eci(FramesFactory.getEME2000()).
99 ecef(FramesFactory.getITRF(IERSConventions.IERS_2010, false)).
100 build();
101
102 final AbsoluteDate date0 = almanacs.get(0).getDate();
103 final Vector3D p0 = propagator.propagateInEcef(date0).getPosition();
104 final double gpsCycleDuration = almanacs.get(0).getCycleDuration();
105 final AbsoluteDate date1 = date0.shiftedBy(gpsCycleDuration);
106 final Vector3D p1 = propagator.propagateInEcef(date1).getPosition();
107
108
109 Assert.assertEquals(0., p0.distance(p1), 0.);
110 }
111
112 @Test
113 public void testFrames() {
114
115 final GNSSPropagator propagator = new GNSSPropagatorBuilder(almanacs.get(0)).build();
116 Assert.assertEquals("EME2000", propagator.getFrame().getName());
117 Assert.assertEquals(3.986005e14, almanacs.get(0).getMu(), 1.0e6);
118
119 final AbsoluteDate date = new AbsoluteDate(2016, 3, 3, 12, 0, 0., TimeScalesFactory.getUTC());
120
121 final PVCoordinates pv0 = propagator.propagateInEcef(date);
122
123 final PVCoordinates pv1 = propagator.getPVCoordinates(date, propagator.getECEF());
124
125
126 Assert.assertEquals(0., pv0.getPosition().distance(pv1.getPosition()), 3.3e-8);
127 Assert.assertEquals(0., pv0.getVelocity().distance(pv1.getVelocity()), 3.9e-12);
128 }
129
130 @Test
131 public void testNoReset() {
132 try {
133 GNSSPropagator propagator = new GNSSPropagatorBuilder(almanacs.get(0)).build();
134 propagator.resetInitialState(propagator.getInitialState());
135 Assert.fail("an exception should have been thrown");
136 } catch (OrekitException oe) {
137 Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier());
138 }
139 try {
140 GNSSPropagator propagator = new GNSSPropagatorBuilder(almanacs.get(0)).build();
141 propagator.resetIntermediateState(propagator.getInitialState(), true);
142 Assert.fail("an exception should have been thrown");
143 } catch (OrekitException oe) {
144 Assert.assertEquals(OrekitMessages.NON_RESETABLE_STATE, oe.getSpecifier());
145 }
146 }
147
148 @Test
149 public void testTLE() {
150
151 List<GNSSPropagator> gpsPropagators = new ArrayList<>();
152 for (final GPSAlmanac almanac : almanacs) {
153 gpsPropagators.add(new GNSSPropagatorBuilder(almanac).build());
154 }
155
156
157 final Map<Integer, TLE> prnToTLE = new HashMap<Integer, TLE>();
158 prnToTLE.put( 1,
159 new TLE("1 37753U 11036A 16059.51505483 -.00000016 00000-0 00000+0 0 9995",
160 "2 37753 55.2230 119.7200 0049958 23.9363 306.3749 2.00566105 33828"));
161 prnToTLE.put( 2,
162 new TLE("1 28474U 04045A 16059.68518942 -.00000018 +00000-0 +00000-0 0 9992",
163 "2 28474 054.0048 117.2153 0156693 236.8152 092.4773 02.00556694082981"));
164 prnToTLE.put( 3,
165 new TLE("1 40294U 14068A 16059.64183862 +.00000012 +00000-0 +00000-0 0 9990",
166 "2 40294 054.9632 179.3690 0003634 223.4011 136.5596 02.00553679009768"));
167 prnToTLE.put( 4,
168 new TLE("1 34661U 09014A 16059.51658765 -.00000072 00000-0 00000+0 0 9997",
169 "2 34661 56.3471 0.6400 0080177 48.0430 129.2467 2.00572451 50844"));
170 prnToTLE.put( 5,
171 new TLE("1 35752U 09043A 16059.39819238 .00000011 00000-0 00000+0 0 9993",
172 "2 35752 54.2243 178.7652 0044753 24.8598 29.4422 2.00555538 47893"));
173 prnToTLE.put( 6,
174 new TLE("1 39741U 14026A 16059.18044747 -.00000016 00000-0 00000+0 0 9990",
175 "2 39741 55.2140 119.2493 0005660 259.2190 100.7882 2.00566982 13061"));
176 prnToTLE.put( 7,
177 new TLE("1 32711U 08012A 16059.36304856 -.00000033 +00000-0 +00000-0 0 9998",
178 "2 32711 055.4269 300.7399 0091867 207.6311 151.9340 02.00564257058321"));
179 prnToTLE.put( 8,
180 new TLE("1 40730U 15033A 16059.44106931 -.00000026 +00000-0 +00000-0 0 9994",
181 "2 40730 055.1388 059.0069 0020452 282.1769 077.6168 02.00566073004562"));
182 prnToTLE.put( 9,
183 new TLE("1 40105U 14045A 16059.27451329 .00000045 00000-0 00000+0 0 9996",
184 "2 40105 54.7529 238.9873 0004485 121.4766 238.5557 2.00568637 11512"));
185 prnToTLE.put(10,
186 new TLE("1 41019U 15062A 16059.49433942 .00000013 00000-0 00000+0 0 9991",
187 "2 41019 54.9785 179.1399 0012328 204.9013 155.0292 2.00561967 2382"));
188 prnToTLE.put(11,
189 new TLE("1 25933U 99055A 16059.51073770 -.00000024 00000-0 00000+0 0 9997",
190 "2 25933 51.3239 98.4815 0159812 86.1576 266.7718 2.00565163120122"));
191 prnToTLE.put(12,
192 new TLE("1 29601U 06052A 16059.62966898 -.00000070 +00000-0 +00000-0 0 9994",
193 "2 29601 056.7445 002.2755 0057667 037.0706 323.3313 02.00552237067968"));
194 prnToTLE.put(13,
195 new TLE("1 24876U 97035A 16059.41696335 .00000046 00000-0 00000+0 0 9998",
196 "2 24876 55.6966 245.8203 0044339 114.8899 245.5712 2.00562657136305"));
197 prnToTLE.put(14,
198 new TLE("1 26605U 00071A 16059.56211888 .00000047 00000-0 00000+0 0 9997",
199 "2 26605 55.2663 243.7251 0085518 248.7231 95.5323 2.00557009112094"));
200 prnToTLE.put(15,
201 new TLE("1 32260U 07047A 16059.45678257 +.00000044 +00000-0 +00000-0 0 9994",
202 "2 32260 053.3641 236.0940 0079746 026.4105 333.9774 02.00547771061402"));
203 prnToTLE.put(16,
204 new TLE("1 27663U 03005A 16059.14440417 -.00000071 00000-0 00000+0 0 9996",
205 "2 27663 56.7743 3.3691 0085346 17.6322 214.4333 2.00559487 95843"));
206 prnToTLE.put(17,
207 new TLE("1 28874U 05038A 16059.21070933 -.00000024 00000-0 00000+0 0 9997",
208 "2 28874 55.8916 61.9596 0112077 248.9647 205.7384 2.00567116 76379"));
209 prnToTLE.put(18,
210 new TLE("1 26690U 01004A 16059.51332910 .00000008 00000-0 00000+0 0 9990",
211 "2 26690 52.9999 177.6630 0169501 250.8579 153.6293 2.00563995110499"));
212 prnToTLE.put(19,
213 new TLE("1 28190U 04009A 16058.12363503 -.00000030 00000-0 00000+0 0 9999",
214 "2 28190 55.7230 64.8110 0105865 40.0254 321.4519 2.00572212 87500"));
215 prnToTLE.put(20,
216 new TLE("1 26360U 00025A 16059.44770263 .00000005 00000-0 00000+0 0 9992",
217 "2 26360 53.0712 174.6895 0046205 76.0615 334.4302 2.00559931115818"));
218 prnToTLE.put(21,
219 new TLE("1 27704U 03010A 16059.50719524 -.00000019 00000-0 00000+0 0 9998",
220 "2 27704 53.6134 117.9454 0234081 255.6874 199.2128 2.00564673 94659"));
221 prnToTLE.put(22,
222 new TLE("1 28129U 03058A 16059.06680941 .00000008 00000-0 00000+0 0 9990",
223 "2 28129 52.8771 177.7253 0079127 245.1376 114.0279 2.00398763 89357"));
224 prnToTLE.put(23,
225 new TLE("1 28361U 04023A 16059.54310021 .00000046 00000-0 00000+0 0 9995",
226 "2 28361 54.2347 239.3240 0106509 211.5355 11.7648 2.00557932 85613"));
227 prnToTLE.put(24,
228 new TLE("1 38833U 12053A 16059.04618549 -.00000032 00000-0 00000+0 0 9999",
229 "2 38833 54.4591 298.1383 0042253 18.7074 341.5041 2.00568407 24895"));
230 prnToTLE.put(25,
231 new TLE("1 36585U 10022A 16059.29300735 -.00000074 00000-0 00000+0 0 9993",
232 "2 36585 56.0738 359.4320 0050768 38.3425 49.1794 2.00578535 42134"));
233 prnToTLE.put(26,
234 new TLE("1 40534U 15013A 16059.28299301 -.00000076 00000-0 00000+0 0 9994",
235 "2 40534 55.0430 359.0082 0009349 342.4081 17.5685 2.00558853 6801"));
236 prnToTLE.put(27,
237 new TLE("1 39166U 13023A 16059.40401153 -.00000025 00000-0 00000+0 0 9990",
238 "2 39166 55.6020 59.1224 0032420 7.7969 352.2759 2.00568484 20414"));
239 prnToTLE.put(28,
240 new TLE("1 26407U 00040A 16059.80383354 -.00000069 +00000-0 +00000-0 0 9994",
241 "2 26407 056.6988 003.6328 0201499 267.0948 317.6209 02.00569902114508"));
242 prnToTLE.put(29,
243 new TLE("1 32384U 07062A 16059.44770263 -.00000021 00000-0 00000+0 0 9992",
244 "2 32384 55.9456 62.5022 0011922 319.9531 172.6730 2.00571577 60128"));
245 prnToTLE.put(30,
246 new TLE("1 39533U 14008A 16059.40267873 -.00000038 +00000-0 +00000-0 0 9996",
247 "2 39533 054.6126 303.3404 0017140 179.4267 180.6311 02.00568364014251"));
248 prnToTLE.put(31,
249 new TLE("1 29486U 06042A 16059.50651990 -.00000032 00000-0 00000+0 0 9992",
250 "2 29486 55.7041 301.2472 0084115 334.2804 254.9897 2.00560606 69098"));
251 prnToTLE.put(32,
252 new TLE("1 41328U 16007A 16059.56873502 .00000049 00000-0 00000+0 0 9991",
253 "2 41328 55.0137 239.0304 0002157 298.9074 61.0768 1.99172830 453"));
254
255 for (final GNSSPropagator gpsPropagator : gpsPropagators) {
256 final int prn = gpsPropagator.getOrbitalElements().getPRN();
257 TLE tle = prnToTLE.get(prn);
258 TLEPropagator tlePropagator = TLEPropagator.selectExtrapolator(tle);
259 for (double dt = 0; dt < Constants.JULIAN_DAY; dt += 600) {
260 final AbsoluteDate date = tlePropagator.getInitialState().getDate().shiftedBy(dt);
261 final PVCoordinates gpsPV = gpsPropagator.getPVCoordinates(date, gpsPropagator.getECI());
262 final PVCoordinates tlePV = tlePropagator.getPVCoordinates(date, gpsPropagator.getECI());
263 Assert.assertEquals(0.0,
264 Vector3D.distance(gpsPV.getPosition(), tlePV.getPosition()),
265 8400.0);
266 }
267 }
268 }
269
270 @Test
271 public void testDerivativesConsistency() {
272
273 final Frame eme2000 = FramesFactory.getEME2000();
274 double errorP = 0;
275 double errorV = 0;
276 double errorA = 0;
277 for (final GPSAlmanac almanac : almanacs) {
278 GNSSPropagator propagator = new GNSSPropagatorBuilder(almanac).build();
279 GNSSOrbitalElements elements = propagator.getOrbitalElements();
280 AbsoluteDate t0 = new GNSSDate(elements.getWeek(), 0.001 * elements.getTime(), SatelliteSystem.GPS).getDate();
281 for (double dt = 0; dt < Constants.JULIAN_DAY; dt += 600) {
282 final AbsoluteDate central = t0.shiftedBy(dt);
283 final PVCoordinates pv = propagator.getPVCoordinates(central, eme2000);
284 final double h = 10.0;
285 List<TimeStampedPVCoordinates> sample = new ArrayList<TimeStampedPVCoordinates>();
286 for (int i = -3; i <= 3; ++i) {
287 sample.add(propagator.getPVCoordinates(central.shiftedBy(i * h), eme2000));
288 }
289 final PVCoordinates interpolated =
290 TimeStampedPVCoordinates.interpolate(central,
291 CartesianDerivativesFilter.USE_P,
292 sample);
293 errorP = FastMath.max(errorP, Vector3D.distance(pv.getPosition(), interpolated.getPosition()));
294 errorV = FastMath.max(errorV, Vector3D.distance(pv.getVelocity(), interpolated.getVelocity()));
295 errorA = FastMath.max(errorA, Vector3D.distance(pv.getAcceleration(), interpolated.getAcceleration()));
296 }
297 }
298 Assert.assertEquals(0.0, errorP, 3.8e-9);
299 Assert.assertEquals(0.0, errorV, 3.5e-8);
300 Assert.assertEquals(0.0, errorA, 1.1e-8);
301
302 }
303
304 @Test
305 public void testPosition() {
306
307 final GPSNavigationMessage goe = new GPSNavigationMessage();
308 goe.setPRN(7);
309 goe.setWeek(0);
310 goe.setTime(288000);
311 goe.setSqrtA(5153.599830627441);
312 goe.setE(0.012442796607501805);
313 goe.setDeltaN(4.419469802942352E-9);
314 goe.setI0(0.9558937988021613);
315 goe.setIDot(-2.4608167886110235E-10);
316 goe.setOmega0(1.0479401362158658);
317 goe.setOmegaDot(-7.967117576712062E-9);
318 goe.setPa(-2.4719019944000538);
319 goe.setM0(-1.0899023379614294);
320 goe.setCuc(4.3995678424835205E-6);
321 goe.setCus(1.002475619316101E-5);
322 goe.setCrc(183.40625);
323 goe.setCrs(87.03125);
324 goe.setCic(3.203749656677246E-7);
325 goe.setCis(4.0978193283081055E-8);
326 goe.setDate(new GNSSDate(goe.getWeek(), 1000.0 * goe.getTime(), SatelliteSystem.GPS).getDate());
327
328
329 final AbsoluteDate target = goe.getDate();
330
331 final GNSSPropagator propagator = new GNSSPropagatorBuilder(goe).build();
332
333 final PVCoordinates pv = propagator.getPVCoordinates(target, FramesFactory.getITRF(IERSConventions.IERS_2010, true));
334
335 final Vector3D computedPos = pv.getPosition();
336
337 final Vector3D expectedPos = new Vector3D(-4920705.292, 24248099.200, 9236130.101);
338
339 Assert.assertEquals(0., Vector3D.distance(expectedPos, computedPos), 3.2);
340 }
341
342 @Test
343 public void testStmAndJacobian() {
344
345 final GNSSPropagator propagator = new GNSSPropagatorBuilder(almanacs.get(0)).
346 attitudeProvider(Utils.defaultLaw()).
347 mass(1521.0).
348 eci(FramesFactory.getEME2000()).
349 ecef(FramesFactory.getITRF(IERSConventions.IERS_2010, false)).
350 build();
351 try {
352 propagator.setupMatricesComputation("stm", null, null);
353 Assert.fail("an exception should have been thrown");
354 } catch (UnsupportedOperationException uoe) {
355
356 }
357 }
358
359 @Test
360 public void testIssue544() {
361
362 final GNSSPropagator propagator = new GNSSPropagatorBuilder(almanacs.get(0)).build();
363
364 final AbsoluteDate date0 = new AbsoluteDate(2010, 5, 7, 7, 50, Double.NaN, TimeScalesFactory.getUTC());
365 final PVCoordinates pv0 = propagator.propagateInEcef(date0);
366
367 Assert.assertEquals(Vector3D.NaN, pv0.getPosition());
368 Assert.assertEquals(Vector3D.NaN, pv0.getVelocity());
369
370 }
371
372 }