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  
18  package org.orekit.estimation.iod;
19  
20  import org.hipparchus.geometry.euclidean.threed.Vector3D;
21  import org.hipparchus.util.FastMath;
22  import org.junit.jupiter.api.Assertions;
23  import org.junit.jupiter.api.Test;
24  
25  import org.orekit.estimation.measurements.AngularAzEl;
26  import org.orekit.orbits.CartesianOrbit;
27  import org.orekit.orbits.KeplerianOrbit;
28  import org.orekit.orbits.Orbit;
29  import org.orekit.orbits.PositionAngleType;
30  import org.orekit.propagation.Propagator;
31  import org.orekit.propagation.analytical.KeplerianPropagator;
32  import org.orekit.time.AbsoluteDate;
33  import org.orekit.time.DateComponents;
34  import org.orekit.time.TimeScalesFactory;
35  import org.orekit.utils.TimeStampedPVCoordinates;
36  import org.orekit.utils.PVCoordinates;
37  import org.orekit.utils.Constants;
38  
39  /**
40   * Data based on "Fundamentals of Astrodynamics and Applications", Vallado
41   * and "On the performance analysis of Initial Orbit Determination algorithms", Juan Carlos Dolado & Carlos Yanez.
42   * @author Asquier Julien
43   * @since 11.3.2
44   */
45  public class IodGaussTest extends AbstractIodTest {
46  
47       /** Value from example of Vallado , example 7.2.
48       * Caution : Values will not be strictly the same as with Vallado results due to difference of use in
49       * EOP. It was verified that the differences came from this. see issue #982 and topic related on Orekit forum. */
50      @Test
51      public void testGaussVallado() {
52  
53          final AbsoluteDate obsDate2 = new AbsoluteDate(2012, 8, 20,
54                                                         11, 48, 28, TimeScalesFactory.getUTC());
55  
56          // Date of the 1st measurement, according to 2nd measurement
57          final AbsoluteDate obsDate1 = obsDate2.shiftedBy(-480);
58          // Date of the 3rd measurement, according to 2nd measurement
59          final AbsoluteDate obsDate3 = obsDate2.shiftedBy(240);
60  
61          // Angular value of the 1st radec
62          final double   ra1  = FastMath.toRadians(0.9399130);
63          final double   dec1 = FastMath.toRadians(18.667717);
64          final double[] ang1 = { ra1, dec1 };
65  
66          // Angular value of the 2nd radec
67          final double   ra2  = FastMath.toRadians(45.025748);
68          final double   dec2 = FastMath.toRadians(35.664741);
69          final double[] ang2 = { ra2, dec2 };
70  
71          // Angular value of the 3rd radec
72          final double   ra3  = FastMath.toRadians(67.886655);
73          final double   dec3 = FastMath.toRadians(36.996583);
74          final double[] ang3 = { ra3, dec3 };
75  
76          // Computation of the lines of sight
77          final Vector3D los1 = new Vector3D(ang1[0], ang1[1]);
78          final Vector3D los2 = new Vector3D(ang2[0], ang2[1]);
79          final Vector3D los3 = new Vector3D(ang3[0], ang3[1]);
80  
81          // Computation of the observer coordinates at the 3 dates, based on the values of the Vallado example 7.2
82          final Vector3D obsP1 = new Vector3D(4054880.1594, 2748194.4767, 4074236.1653);
83          final Vector3D obsP2 = new Vector3D(3956223.5179, 2888232.0864, 4074363.4118);
84          final Vector3D obsP3 = new Vector3D(3905072.3452, 2956934.5902, 4074429.3009);
85  
86          // computation of the estimated orbit based on Gauss method
87          final IodGauss iodgauss = new IodGauss(mu);
88          final Orbit estimatedOrbit = iodgauss.estimate(eme2000, obsP1, obsDate1, los1,
89                                                         obsP2, obsDate2, los2, obsP3, obsDate3, los3);
90  
91          // Expected results from Vallado example, with Orekit EOP (difference existing from the Vallado example)
92          final PVCoordinates pvOrbit = estimatedOrbit.getPVCoordinates();
93          Assertions.assertEquals(6313395.577554352, pvOrbit.getPosition().getX(), 10E-12);
94          Assertions.assertEquals(5247523.665414684, pvOrbit.getPosition().getY(), 10E-12);
95          Assertions.assertEquals(6467724.780943674, pvOrbit.getPosition().getZ(), 10E-12);
96  
97          Assertions.assertEquals(-4101.302835071493, pvOrbit.getVelocity().getX(), 10E-12);
98          Assertions.assertEquals(4699.692868088214, pvOrbit.getVelocity().getY(), 10E-12);
99          Assertions.assertEquals(1692.5478178378698, pvOrbit.getVelocity().getZ(), 10E-12);
100     }
101 
102     /** Non-regression test case based on LEO-1 case:
103      * "On the performance analysis of Initial Orbit Determination algorithms" with unnoisy data. We have better
104      * results on 10^-1 order of magnitude compared to the relative error results of the paper.
105      */
106     @Test
107     public void testGaussLeoSSO() {
108 
109         final DateComponents dateComp = new DateComponents(DateComponents.FIFTIES_EPOCH, 21915);
110         final AbsoluteDate   obsDate2 = new AbsoluteDate(dateComp, TimeScalesFactory.getUTC());
111         // taking the value of the LEO-1 case, and doing a keplerian propagation
112         final KeplerianOrbit kepOrbitRef = new KeplerianOrbit(7197934.7, 0., FastMath.toRadians(98.71),
113                                                               0., FastMath.toRadians(100.41), 0.,
114                                                               PositionAngleType.MEAN, gcrf, obsDate2, Constants.WGS84_EARTH_MU);
115         final KeplerianPropagator propRef = new KeplerianPropagator(kepOrbitRef);
116 
117         // computation of the estimated orbits for 3 different time of propagation
118         final Orbit estimatedOrbit1 = getGaussEstimation(-4, 4, obsDate2, propRef);
119         final Orbit estimatedOrbit2 = getGaussEstimation(-37, 37, obsDate2, propRef);
120         final Orbit estimatedOrbit3 = getGaussEstimation(-90, 90, obsDate2, propRef);
121 
122         //  Computation of the relative errors
123         final double relativeRangeError1    = getRelativeRangeError(estimatedOrbit1, kepOrbitRef);
124         final double relativeVelocityError1 = getRelativeVelocityError(estimatedOrbit1, kepOrbitRef);
125         final double relativeRangeError2    = getRelativeRangeError(estimatedOrbit2, kepOrbitRef);
126         final double relativeVelocityError2 = getRelativeVelocityError(estimatedOrbit2, kepOrbitRef);
127         final double relativeRangeError3    = getRelativeRangeError(estimatedOrbit3, kepOrbitRef);
128         final double relativeVelocityError3 = getRelativeVelocityError(estimatedOrbit3, kepOrbitRef);
129 
130         Assertions.assertEquals(0, relativeRangeError1, 10E-6);
131         Assertions.assertEquals(0, relativeVelocityError1, 10E-6);
132 
133         Assertions.assertEquals(0, relativeRangeError2, 10E-4);
134         Assertions.assertEquals(0, relativeVelocityError2, 10E-4);
135 
136         Assertions.assertEquals(0, relativeRangeError3, 10E-3);
137         Assertions.assertEquals(0, relativeVelocityError3, 10E-3);
138 
139     }
140 
141     /**   Non-regression test case based on MEO case:
142      *   "On the performance analysis of Initial Orbit Determination algorithms" with unnoisy data. We have better
143      *   results on 10^-1 order of magnitude compared to the relative error results of the paper.
144      */
145     @Test
146     public void testGaussMEO() {
147 
148         final DateComponents dateComp = new DateComponents(DateComponents.FIFTIES_EPOCH, 21915);
149         final AbsoluteDate   obsDate2 = new AbsoluteDate(dateComp, TimeScalesFactory.getUTC());
150         final KeplerianOrbit kepOrbitRef = new KeplerianOrbit(29600136., 0., FastMath.toRadians(56.),
151                                                               0., FastMath.toRadians(55.41), 0.,
152                                                               PositionAngleType.MEAN, gcrf, obsDate2, Constants.WGS84_EARTH_MU);
153         // taking the value of the MEO case, and doing a keplerian propagation
154         final KeplerianPropagator propRef = new KeplerianPropagator(kepOrbitRef);
155 
156         // computation of the estimated orbits for 3 different time of propagation
157         final Orbit estimatedOrbit1 = getGaussEstimation(-35, 35, obsDate2, propRef);
158         final Orbit estimatedOrbit2 = getGaussEstimation(-305, 305, obsDate2, propRef);
159         final Orbit estimatedOrbit3 = getGaussEstimation(-760, 760, obsDate2, propRef);
160 
161         // computation of the relative errors
162         final double relativeRangeError1    = getRelativeRangeError(estimatedOrbit1, kepOrbitRef);
163         final double relativeVelocityError1 = getRelativeVelocityError(estimatedOrbit1, kepOrbitRef);
164         final double relativeRangeError2    = getRelativeRangeError(estimatedOrbit2, kepOrbitRef);
165         final double relativeVelocityError2 = getRelativeVelocityError(estimatedOrbit2, kepOrbitRef);
166         final double relativeRangeError3    = getRelativeRangeError(estimatedOrbit3, kepOrbitRef);
167         final double relativeVelocityError3 = getRelativeVelocityError(estimatedOrbit3, kepOrbitRef);
168 
169         Assertions.assertEquals(0, relativeRangeError1, 10E-6);
170         Assertions.assertEquals(0, relativeVelocityError1, 10E-6);
171 
172         Assertions.assertEquals(0, relativeRangeError2, 10E-4);
173         Assertions.assertEquals(0, relativeVelocityError2, 10E-4);
174 
175         Assertions.assertEquals(0, relativeRangeError3, 10E-3);
176         Assertions.assertEquals(0, relativeVelocityError3, 10E-3);
177 
178     }
179 
180     /** Non-regression test case based on GEO-1 case:
181      * "On the performance analysis of Initial Orbit Determination algorithms" with unnoisy data. We have better
182      * results on 10^-1 order of magnitude compared to the relative error results of the paper.
183      */
184     @Test
185     public void testGaussGEO() {
186 
187         final DateComponents dateComp = new DateComponents(DateComponents.FIFTIES_EPOCH, 21915);
188         final AbsoluteDate   obsDate2 = new AbsoluteDate(dateComp, TimeScalesFactory.getUTC());
189         final KeplerianOrbit kepOrbitRef = new KeplerianOrbit(42164000., 0., 0., 0.,
190                                                               FastMath.toRadians(107.33), 0., PositionAngleType.MEAN, gcrf,
191                                                               obsDate2, mu);
192         // taking the value of the GEO case, and doing a keplerian propagation
193         final KeplerianPropagator propRef    = new KeplerianPropagator(kepOrbitRef);
194 
195         // computation of the estimated orbits for 3 different time of propagation
196         final Orbit estimatedOrbit1 = getGaussEstimation(-60, 60, obsDate2, propRef);
197         final Orbit estimatedOrbit2 = getGaussEstimation(-520, 520, obsDate2, propRef);
198         final Orbit estimatedOrbit3 = getGaussEstimation(-1300, 1300, obsDate2, propRef);
199 
200         // computation of the relative errors
201         final double relativeRangeError1    = getRelativeRangeError(estimatedOrbit1, kepOrbitRef);
202         final double relativeVelocityError1 = getRelativeVelocityError(estimatedOrbit1, kepOrbitRef);
203         final double relativeRangeError2    = getRelativeRangeError(estimatedOrbit2, kepOrbitRef);
204         final double relativeVelocityError2 = getRelativeVelocityError(estimatedOrbit2, kepOrbitRef);
205         final double relativeRangeError3    = getRelativeRangeError(estimatedOrbit3, kepOrbitRef);
206         final double relativeVelocityError3 = getRelativeVelocityError(estimatedOrbit3, kepOrbitRef);
207 
208         Assertions.assertEquals(0, relativeRangeError1, 10E-6);
209         Assertions.assertEquals(0, relativeVelocityError1, 10E-6);
210 
211         Assertions.assertEquals(0, relativeRangeError2, 10E-4);
212         Assertions.assertEquals(0, relativeVelocityError2, 10E-4);
213 
214         Assertions.assertEquals(0, relativeRangeError3, 10E-3);
215         Assertions.assertEquals(0, relativeVelocityError3, 10E-3);
216 
217     }
218 
219     /** Non-regression test case comparing the results between the different IOD methods. The relative error taking as
220      * a reference the other IOD is giving an error of the position and velocity with an approximation < 10^-2. Gooding
221      * was not made due to not having good tuning parameters
222      */
223     @Test
224     public void testGaussComparisonLeoSSO() {
225         final DateComponents dateComp = new DateComponents(DateComponents.FIFTIES_EPOCH, 21915);
226         final AbsoluteDate   obsDate2 = new AbsoluteDate(dateComp, TimeScalesFactory.getUTC());
227         final KeplerianOrbit kepOrbitRef = new KeplerianOrbit(7197934.0, 0., FastMath.toRadians(98.71),
228                                                               0., FastMath.toRadians(100.41), 0.,
229                                                               PositionAngleType.MEAN, gcrf, obsDate2, Constants.WGS84_EARTH_MU);
230         final KeplerianPropagator propRef    = new KeplerianPropagator(kepOrbitRef);
231 
232         // Date of the 1st measurement, according to 2nd measurement
233         final AbsoluteDate obsDate1 = obsDate2.shiftedBy(-120);
234 
235         // Date of the 3rd measurement, according to 2nd measurement
236         final AbsoluteDate obsDate3 = obsDate2.shiftedBy(120);
237 
238         // Computation of the LOS angles
239         // Computation of the observer coordinates
240         final Vector3D obsP1 = observer.getBaseFrame().getPosition(obsDate1, gcrf);
241         final TimeStampedPVCoordinates obsP2 = observer.getBaseFrame().getPVCoordinates(obsDate2, gcrf);
242         final Vector3D obsP3 = observer.getBaseFrame().getPosition(obsDate3, gcrf);
243 
244         final Vector3D los1 = getLOSAngles(propRef, obsDate1);
245         final Vector3D los2 = getLOSAngles(propRef, obsDate2);
246         final Vector3D los3 = getLOSAngles(propRef, obsDate3);
247 
248         // Computation of the estimated orbit with iod gauss
249         final IodGauss iodgauss = new IodGauss(mu);
250         final Orbit estimatedGauss = iodgauss.estimate(eme2000, obsP1, obsDate1, los1,
251                                                        obsP2.getPosition(), obsDate2, los2, obsP3, obsDate3, los3);
252         // Computation of the estimated orbit with iod laplace
253         final IodLaplace iodLaplace = new IodLaplace(Constants.WGS84_EARTH_MU);
254         final Orbit estimatedLaplace = iodLaplace.estimate(eme2000, obsP2, obsDate1,
255                                                            los1, obsDate2, los2, obsDate3, los3);
256         // Computation of the estimated orbit with iod lambert
257         final IodLambert iodLambert = new IodLambert(Constants.WGS84_EARTH_MU);
258         final Orbit estimatedKepLambert = iodLambert.estimate(eme2000, true, 0,
259                                                               propRef.getPosition(obsDate1, eme2000), obsDate1,
260                                                               propRef.getPosition(obsDate2,
261                                                                                   eme2000), obsDate2);
262         final Orbit estimatedLambert = new CartesianOrbit(estimatedKepLambert);
263         // Computation of the estimated orbit with iod gibbs
264         final IodGibbs iodGibbs = new IodGibbs(Constants.WGS84_EARTH_MU);
265         final Orbit estimatedKepGibbs = iodGibbs.estimate(eme2000, propRef.getPosition(obsDate1, eme2000),
266                                                           obsDate1, propRef.getPosition(obsDate2, eme2000), obsDate2,
267                                                           propRef.getPosition(obsDate3, eme2000), obsDate3);
268         final Orbit estimatedGibbs = new CartesianOrbit(estimatedKepGibbs);
269 
270         // Relative error with reference keplerian propagation
271         final double relativeRangeError1    = getRelativeRangeError(estimatedGauss, kepOrbitRef);
272         final double relativeVelocityError1 = getRelativeVelocityError(estimatedGauss, kepOrbitRef);
273 
274         // Relative error with reference Gibbs estimation
275         final double relativeRangeError2    = getRelativeRangeError(estimatedGauss, estimatedGibbs);
276         final double relativeVelocityError2 = getRelativeVelocityError(estimatedGauss, estimatedGibbs);
277 
278         // Relative error with reference Laplace estimation
279         final double relativeRangeError3    = getRelativeRangeError(estimatedGauss, estimatedLaplace);
280         final double relativeVelocityError3 = getRelativeVelocityError(estimatedGauss, estimatedLaplace);
281 
282         // Relative error with reference Lambert estimation
283         final double relativeRangeError4    = getRelativeRangeError(estimatedGauss, estimatedLambert);
284         final double relativeVelocityError4 = getRelativeVelocityError(estimatedGauss, estimatedLambert);
285 
286         Assertions.assertEquals(0, relativeRangeError1, 10E-2);
287         Assertions.assertEquals(0, relativeVelocityError1, 10E-2);
288 
289         Assertions.assertEquals(0, relativeRangeError2, 10E-2);
290         Assertions.assertEquals(0, relativeVelocityError2, 10E-2);
291 
292         Assertions.assertEquals(0, relativeRangeError3, 10E-2);
293         Assertions.assertEquals(0, relativeVelocityError3, 10E-2);
294 
295         Assertions.assertEquals(0, relativeRangeError4, 10E-2);
296         Assertions.assertEquals(0, relativeVelocityError4, 10E-2);
297 
298     }
299 
300     @Test
301     public void testLaplaceKeplerianWithAzEl() {
302         // Settings
303         final AbsoluteDate date = new AbsoluteDate(2019, 9, 29, 22, 0, 2.0, TimeScalesFactory.getUTC());
304         final KeplerianOrbit kep = new KeplerianOrbit(6798938.970424857, 0.0021115522920270016, 0.9008866630545347,
305             1.8278985811406743, -2.7656136723308524,
306             0.8823034512437679, PositionAngleType.MEAN, gcrf,
307             date, Constants.EGM96_EARTH_MU);
308         final KeplerianPropagator prop = new KeplerianPropagator(kep);
309 
310         // Measurements
311         final AngularAzEl azEl1 = getAzEl(prop, date);
312         final AngularAzEl azEl2 = getAzEl(prop, date.shiftedBy(5.0));
313         final AngularAzEl azEl3 = getAzEl(prop, date.shiftedBy(10.0));
314 
315         // With only 3 measurements, we can expect ~400 meters error in position and ~1 m/s in velocity
316         final Orbit estOrbit = new IodGauss(Constants.EGM96_EARTH_MU).estimate(gcrf, azEl1, azEl2, azEl3);
317 
318         // Verify
319         final TimeStampedPVCoordinates truth = prop.getPVCoordinates(azEl2.getDate(), gcrf);
320         Assertions.assertEquals(0.0, Vector3D.distance(truth.getPosition(), estOrbit.getPosition()), 262.0);
321         Assertions.assertEquals(0.0, Vector3D.distance(truth.getVelocity(), estOrbit.getPVCoordinates().getVelocity()), 0.3);
322     }
323 
324     // Private method to have a gauss estimated orbit
325     private Orbit getGaussEstimation(final double deltaT1, final double deltaT3, final AbsoluteDate obsDate2,
326                                      final Propagator prop) {
327 
328         // Date of the 1st measurement and 3rd, according to 2nd measurement
329         final AbsoluteDate obsDate1 = obsDate2.shiftedBy(deltaT1);
330         final AbsoluteDate obsDate3 = obsDate2.shiftedBy(deltaT3);
331 
332         // Computation of the 3 LOS
333         final Vector3D los1 = getLOSAngles(prop, obsDate1);
334         final Vector3D los2 = getLOSAngles(prop, obsDate2);
335         final Vector3D los3 = getLOSAngles(prop, obsDate3);
336 
337         // Computation of the observer coordinates at the 3 times of measurements
338         final Vector3D obsPva  = observer.getBaseFrame().getPosition(obsDate1, gcrf);
339         final Vector3D obsPva2 = observer.getBaseFrame().getPosition(obsDate2, gcrf);
340         final Vector3D obsPva3 = observer.getBaseFrame().getPosition(obsDate3, gcrf);
341 
342         // Gauss estimation
343         final IodGauss iodgauss = new IodGauss(mu);
344         return iodgauss.estimate(gcrf, obsPva, obsDate1, los1, obsPva2, obsDate2, los2, obsPva3, obsDate3, los3);
345 
346     }
347 
348 }