1   /* Copyright 2002-2025 CS GROUP
2    * Licensed to CS GROUP (CS) under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * CS licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *   http://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
16   */
17  package org.orekit.propagation.analytical.gnss;
18  
19  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
20  import org.hipparchus.geometry.euclidean.threed.Vector3D;
21  import org.hipparchus.util.Binary64;
22  import org.hipparchus.util.Binary64Field;
23  import org.hipparchus.util.FastMath;
24  import org.junit.jupiter.api.Assertions;
25  import org.junit.jupiter.api.BeforeAll;
26  import org.junit.jupiter.api.BeforeEach;
27  import org.junit.jupiter.api.Test;
28  import org.orekit.Utils;
29  import org.orekit.data.DataContext;
30  import org.orekit.frames.Frame;
31  import org.orekit.frames.FramesFactory;
32  import org.orekit.gnss.SatelliteSystem;
33  import org.orekit.propagation.SpacecraftState;
34  import org.orekit.propagation.analytical.gnss.data.FieldGalileoAlmanac;
35  import org.orekit.propagation.analytical.gnss.data.GNSSOrbitalElements;
36  import org.orekit.propagation.analytical.gnss.data.GalileoAlmanac;
37  import org.orekit.propagation.analytical.gnss.data.GalileoNavigationMessage;
38  import org.orekit.time.AbsoluteDate;
39  import org.orekit.time.FieldAbsoluteDate;
40  import org.orekit.time.GNSSDate;
41  import org.orekit.time.TimeInterpolator;
42  import org.orekit.time.TimeScalesFactory;
43  import org.orekit.utils.CartesianDerivativesFilter;
44  import org.orekit.utils.Constants;
45  import org.orekit.utils.IERSConventions;
46  import org.orekit.utils.PVCoordinates;
47  import org.orekit.utils.TimeStampedPVCoordinates;
48  import org.orekit.utils.TimeStampedPVCoordinatesHermiteInterpolator;
49  
50  import java.util.ArrayList;
51  import java.util.List;
52  
53  public class GalileoPropagatorTest {
54  
55      private GalileoNavigationMessage goe;
56  
57      @BeforeEach
58      public void setUp() {
59          goe = new GalileoNavigationMessage(DataContext.getDefault().getTimeScales(),
60                                             SatelliteSystem.GALILEO);
61          goe.setPRN(4);
62          goe.setWeek(1024);
63          goe.setTime(293400.0);
64          goe.setSqrtA(5440.602949142456);
65          goe.setDeltaN0(3.7394414770330066E-9);
66          goe.setE(2.4088891223073006E-4);
67          goe.setI0(0.9531656087278083);
68          goe.setIDot(-2.36081262303612E-10);
69          goe.setOmega0(-0.36639513583951266);
70          goe.setOmegaDot(-5.7695260382035525E-9);
71          goe.setPa(-1.6870064194345724);
72          goe.setM0(-0.38716557650888);
73          goe.setCuc(-8.903443813323975E-7);
74          goe.setCus(6.61797821521759E-6);
75          goe.setCrc(194.0625);
76          goe.setCrs(-18.78125);
77          goe.setCic(3.166496753692627E-8);
78          goe.setCis(-1.862645149230957E-8);
79          goe.setEpochToc(new GNSSDate(1024, 0.0, SatelliteSystem.GALILEO).getDate());
80      }
81  
82      @BeforeAll
83      public static void setUpBeforeClass() {
84          Utils.setDataRoot("gnss");
85      }
86  
87      @Test
88      public void testGalileoCycle() {
89          // Reference for the almanac: 2019-05-28T09:40:01.0Z
90          final GalileoAlmanac almanac = new GalileoAlmanac(DataContext.getDefault().getTimeScales(),
91                                                            SatelliteSystem.GALILEO);
92          almanac.setPRN(1);
93          almanac.setWeek(1024);
94          almanac.setTime(293400.0);
95          almanac.setDeltaSqrtA(0.013671875);
96          almanac.setE(0.000152587890625);
97          almanac.setDeltaInc(0.003356933593);
98          almanac.setIOD(4);
99          almanac.setOmega0(0.2739257812499857891);
100         almanac.setOmegaDot(-1.74622982740407E-9);
101         almanac.setPa(0.7363586425);
102         almanac.setM0(0.27276611328124);
103         almanac.setAf0(-0.0006141662597);
104         almanac.setAf1(-7.275957614183E-12);
105         almanac.setHealthE1(0);
106         almanac.setHealthE5a(0);
107         almanac.setHealthE5b(0);
108 
109         // Intermediate verification
110         Assertions.assertEquals(1,                   almanac.getPRN());
111         Assertions.assertEquals(1024,                almanac.getWeek());
112         Assertions.assertEquals(4,                   almanac.getIOD());
113         Assertions.assertEquals(0,                   almanac.getHealthE1());
114         Assertions.assertEquals(0,                   almanac.getHealthE5a());
115         Assertions.assertEquals(0,                   almanac.getHealthE5b());
116         Assertions.assertEquals(-0.0006141662597,    almanac.getAf0(), 1.0e-15);
117         Assertions.assertEquals(-7.275957614183E-12, almanac.getAf1(), 1.0e-15);
118 
119         // Builds the GalileoPropagator from the almanac
120         final GNSSPropagator propagator = almanac.getPropagator();
121         // Propagate at the Galileo date and one Galileo cycle later
122         final AbsoluteDate date0 = almanac.getDate();
123         final Vector3D p0 = propagator.propagateInEcef(date0).getPosition();
124         final double galCycleDuration = almanac.getCycleDuration();
125         final AbsoluteDate date1 = date0.shiftedBy(galCycleDuration);
126         final Vector3D p1 = propagator.propagateInEcef(date1).getPosition();
127 
128         // Checks
129         Assertions.assertEquals(0., p0.distance(p1), 0.);
130     }
131 
132     @Test
133     public void testFieldGalileoCycle() {
134         // Reference for the almanac: 2019-05-28T09:40:01.0Z
135         final FieldGalileoAlmanac<Binary64> almanac =
136             new GalileoAlmanac(DataContext.getDefault().getTimeScales(),
137                                SatelliteSystem.GALILEO).toField(Binary64Field.getInstance());
138         almanac.setPRN(1);
139         almanac.setWeek(1024);
140         almanac.setTime(293400.0);
141         almanac.setDeltaSqrtA(new Binary64(0.013671875));
142         almanac.setE(new Binary64(0.000152587890625));
143         almanac.setDeltaInc(new Binary64(0.003356933593));
144         almanac.setIOD(4);
145         almanac.setOmega0(new Binary64(0.2739257812499857891));
146         almanac.setOmegaDot(-1.74622982740407E-9);
147         almanac.setPa(new Binary64(0.7363586425));
148         almanac.setM0(new Binary64(0.27276611328124));
149         almanac.setAf0(new Binary64(-0.0006141662597));
150         almanac.setAf1(new Binary64(-7.275957614183E-12));
151         almanac.setHealthE1(0);
152         almanac.setHealthE5a(0);
153         almanac.setHealthE5b(0);
154 
155         // Intermediate verification
156         Assertions.assertEquals(1,                   almanac.getPRN());
157         Assertions.assertEquals(1024,                almanac.getWeek());
158         Assertions.assertEquals(4,                   almanac.getIOD());
159         Assertions.assertEquals(0,                   almanac.getHealthE1());
160         Assertions.assertEquals(0,                   almanac.getHealthE5a());
161         Assertions.assertEquals(0,                   almanac.getHealthE5b());
162         Assertions.assertEquals(-0.0006141662597,    almanac.getAf0().getReal(), 1.0e-15);
163         Assertions.assertEquals(-7.275957614183E-12, almanac.getAf1().getReal(), 1.0e-15);
164 
165         // Builds the GalileoPropagator from the almanac
166         final FieldGnssPropagator<Binary64> propagator = almanac.getPropagator();
167         // Propagate at the Galileo date and one Galileo cycle later
168         final FieldAbsoluteDate<Binary64> date0 = almanac.getDate();
169         final FieldVector3D<Binary64> p0 =
170                 propagator.propagateInEcef(date0, propagator.getParameters(Binary64Field.getInstance())).
171                 getPosition();
172         final double galCycleDuration = almanac.getCycleDuration();
173         final FieldAbsoluteDate<Binary64> date1 = date0.shiftedBy(galCycleDuration);
174         final FieldVector3D<Binary64> p1 =
175                 propagator.propagateInEcef(date1, propagator.getParameters(Binary64Field.getInstance())).
176                 getPosition();
177 
178         // Checks
179         Assertions.assertEquals(0., p0.distance(p1).getReal(), 0.);
180     }
181 
182     @Test
183     public void testFrames() {
184         // Builds the GalileoPropagator from the ephemeris
185         final GNSSPropagator propagator = goe.getPropagator();
186         Assertions.assertEquals("EME2000", propagator.getFrame().getName());
187         Assertions.assertEquals(3.986004418e+14, goe.getMu(), 1.0e6);
188         // Defines some date
189         final AbsoluteDate date = new AbsoluteDate(2016, 3, 3, 12, 0, 0., TimeScalesFactory.getUTC());
190         // Get PVCoordinates at the date in the ECEF
191         final PVCoordinates pv0 = propagator.propagateInEcef(date);
192         // Get PVCoordinates at the date in the ECEF
193         final PVCoordinates pv1 = propagator.getPVCoordinates(date, propagator.getECEF());
194 
195         // Checks
196         Assertions.assertEquals(0., pv0.getPosition().distance(pv1.getPosition()), 2.5e-8);
197         Assertions.assertEquals(0., pv0.getVelocity().distance(pv1.getVelocity()), 2.8e-12);
198     }
199 
200     @Test
201     public void testResetInitialState() {
202         final GNSSPropagator propagator = goe.getPropagator();
203         final SpacecraftState old = propagator.getInitialState();
204         propagator.resetInitialState(new SpacecraftState(old.getOrbit(), old.getAttitude(), old.getMass() + 1000));
205         Assertions.assertEquals(old.getMass() + 1000, propagator.getInitialState().getMass(), 1.0e-9);
206     }
207 
208     @Test
209     public void testResetIntermediateState() {
210         GNSSPropagator propagator = new GNSSPropagatorBuilder(goe).build();
211         final SpacecraftState old = propagator.getInitialState();
212         propagator.resetIntermediateState(new SpacecraftState(old.getOrbit(), old.getAttitude(), old.getMass() + 1000),
213                                           true);
214         Assertions.assertEquals(old.getMass() + 1000, propagator.getInitialState().getMass(), 1.0e-9);
215     }
216 
217     @Test
218     public void testDerivativesConsistency() {
219 
220         final Frame eme2000 = FramesFactory.getEME2000();
221         double errorP = 0;
222         double errorV = 0;
223         double errorA = 0;
224         final GNSSPropagator propagator = goe.getPropagator();
225         GNSSOrbitalElements<?> elements = propagator.getOrbitalElements();
226         AbsoluteDate t0 = new GNSSDate(elements.getWeek(), elements.getTime(), SatelliteSystem.GALILEO).getDate();
227         for (double dt = 0; dt < Constants.JULIAN_DAY; dt += 600) {
228             final AbsoluteDate central = t0.shiftedBy(dt);
229             final PVCoordinates pv = propagator.getPVCoordinates(central, eme2000);
230             final double h = 60.0;
231             List<TimeStampedPVCoordinates> sample = new ArrayList<>();
232             for (int i = -3; i <= 3; ++i) {
233                 sample.add(propagator.getPVCoordinates(central.shiftedBy(i * h), eme2000));
234             }
235 
236             // create interpolator
237             final TimeInterpolator<TimeStampedPVCoordinates> interpolator =
238                     new TimeStampedPVCoordinatesHermiteInterpolator(sample.size(), CartesianDerivativesFilter.USE_P);
239 
240             final PVCoordinates interpolated = interpolator.interpolate(central, sample);
241             errorP = FastMath.max(errorP, Vector3D.distance(pv.getPosition(), interpolated.getPosition()));
242             errorV = FastMath.max(errorV, Vector3D.distance(pv.getVelocity(), interpolated.getVelocity()));
243             errorA = FastMath.max(errorA, Vector3D.distance(pv.getAcceleration(), interpolated.getAcceleration()));
244         }
245         Assertions.assertEquals(0.0, errorP, 1.9e-9);
246         Assertions.assertEquals(0.0, errorV, 4.4e-8);
247         Assertions.assertEquals(0.0, errorA, 1.8e-9);
248 
249     }
250 
251     @Test
252     public void testPosition() {
253         // Date of the Galileo orbital elements, 10 April 2019 at 09:30:00 UTC
254         final AbsoluteDate target = goe.getDate();
255         // Build the Galileo propagator
256         final GNSSPropagator propagator = goe.getPropagator();
257         // Compute the PV coordinates at the date of the Galileo orbital elements
258         final PVCoordinates pv = propagator.getPVCoordinates(target, FramesFactory.getITRF(IERSConventions.IERS_2010, true));
259         // Computed position
260         final Vector3D computedPos = pv.getPosition();
261         // Expected position (reference from IGS file WUM0MGXULA_20191010500_01D_15M_ORB.sp3)
262         final Vector3D expectedPos = new Vector3D(10487480.721, 17867448.753, -21131462.002);
263         Assertions.assertEquals(0., Vector3D.distance(expectedPos, computedPos), 2.1);
264     }
265 
266     @Test
267     public void testIssue544() {
268         // Builds the GalileoPropagator from the almanac
269         final GNSSPropagator propagator = goe.getPropagator();
270         // In order to test the issue, we voluntary set a Double.NaN value in the date.
271         final AbsoluteDate date0 = new AbsoluteDate(2010, 5, 7, 7, 50, Double.NaN, TimeScalesFactory.getUTC());
272         final PVCoordinates pv0 = propagator.propagateInEcef(date0);
273         // Verify that an infinite loop did not occur
274         Assertions.assertEquals(Vector3D.NaN, pv0.getPosition());
275         Assertions.assertEquals(Vector3D.NaN, pv0.getVelocity());
276     }
277 
278     @Test
279     public void testConversion() {
280         GnssTestUtils.checkFieldConversion(goe);
281     }
282 
283 }