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;
18  
19  import org.hipparchus.analysis.polynomials.SmoothStepFactory;
20  import org.hipparchus.geometry.euclidean.threed.Vector3D;
21  import org.hipparchus.linear.BlockRealMatrix;
22  import org.hipparchus.linear.RealMatrix;
23  import org.hipparchus.ode.ODEIntegrator;
24  import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
25  import org.hipparchus.stat.descriptive.DescriptiveStatistics;
26  import org.hipparchus.util.FastMath;
27  import org.junit.jupiter.api.Assertions;
28  import org.junit.jupiter.api.BeforeAll;
29  import org.junit.jupiter.api.DisplayName;
30  import org.junit.jupiter.api.Test;
31  import org.orekit.Utils;
32  import org.orekit.bodies.BodyShape;
33  import org.orekit.bodies.CelestialBody;
34  import org.orekit.bodies.CelestialBodyFactory;
35  import org.orekit.bodies.OneAxisEllipsoid;
36  import org.orekit.data.DataContext;
37  import org.orekit.forces.drag.DragForce;
38  import org.orekit.forces.drag.IsotropicDrag;
39  import org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel;
40  import org.orekit.forces.gravity.OceanTides;
41  import org.orekit.forces.gravity.Relativity;
42  import org.orekit.forces.gravity.SolidTides;
43  import org.orekit.forces.gravity.ThirdBodyAttraction;
44  import org.orekit.forces.gravity.potential.AstronomicalAmplitudeReader;
45  import org.orekit.forces.gravity.potential.EGMFormatReader;
46  import org.orekit.forces.gravity.potential.FESCHatEpsilonReader;
47  import org.orekit.forces.gravity.potential.GravityFieldFactory;
48  import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider;
49  import org.orekit.forces.gravity.potential.OceanLoadDeformationCoefficients;
50  import org.orekit.forces.radiation.IsotropicRadiationSingleCoefficient;
51  import org.orekit.forces.radiation.KnockeRediffusedForceModel;
52  import org.orekit.forces.radiation.RadiationSensitive;
53  import org.orekit.forces.radiation.SolarRadiationPressure;
54  import org.orekit.frames.Frame;
55  import org.orekit.frames.FramesFactory;
56  import org.orekit.frames.LOFType;
57  import org.orekit.models.earth.ReferenceEllipsoid;
58  import org.orekit.models.earth.atmosphere.Atmosphere;
59  import org.orekit.models.earth.atmosphere.NRLMSISE00;
60  import org.orekit.models.earth.atmosphere.data.CssiSpaceWeatherData;
61  import org.orekit.orbits.CartesianOrbit;
62  import org.orekit.orbits.Orbit;
63  import org.orekit.orbits.OrbitBlender;
64  import org.orekit.orbits.OrbitType;
65  import org.orekit.orbits.PositionAngleType;
66  import org.orekit.propagation.analytical.Ephemeris;
67  import org.orekit.propagation.analytical.KeplerianPropagator;
68  import org.orekit.propagation.numerical.NumericalPropagator;
69  import org.orekit.time.AbsoluteDate;
70  import org.orekit.time.AbstractTimeInterpolator;
71  import org.orekit.time.TimeInterpolator;
72  import org.orekit.time.TimeScalesFactory;
73  import org.orekit.time.TimeStampedPair;
74  import org.orekit.utils.CartesianDerivativesFilter;
75  import org.orekit.utils.Constants;
76  import org.orekit.utils.IERSConventions;
77  import org.orekit.utils.PVCoordinates;
78  
79  import java.util.ArrayList;
80  import java.util.List;
81  import java.util.Locale;
82  import java.util.Map;
83  
84  public class StateCovarianceKeplerianHermiteInterpolatorTest {
85      private static Orbit  sergeiOrbit;
86      private static Frame  sergeiFrame;
87      private final  int    DEFAULT_SERGEI_INTERPOLATION_POINTS = 2;
88      private final  double DEFAULT_SERGEI_PROPAGATION_TIME     = 2400;
89      private final  double DEFAULT_SERGEI_TABULATED_TIMESTEP   = 2400;
90  
91      @BeforeAll
92      public static void setUp() {
93          Utils.setDataRoot("regular-data:potential/egm-format:atmosphere:tides:regular-data/de405-ephemerides");
94          GravityFieldFactory.addPotentialCoefficientsReader(new EGMFormatReader("EGM96-truncated-21x21", true));
95          AstronomicalAmplitudeReader aaReader =
96                  new AstronomicalAmplitudeReader("hf-fes2004.dat", 5, 2, 3, 1.0);
97          DataContext.getDefault().getDataProvidersManager().feed(aaReader.getSupportedNames(), aaReader);
98          Map<Integer, Double> map = aaReader.getAstronomicalAmplitudesMap();
99          GravityFieldFactory.addOceanTidesReader(new FESCHatEpsilonReader("fes2004-7x7.dat",
100                                                                          0.01, FastMath.toRadians(1.0),
101                                                                          OceanLoadDeformationCoefficients.IERS_2010,
102                                                                          map));
103 
104         sergeiOrbit = generateSergeiReferenceOrbit();
105         sergeiFrame = sergeiOrbit.getFrame();
106     }
107 
108     public static DescriptiveStatistics[] computeStatisticsCovarianceLOFInterpolation(final double propagationDuration,
109                                                                                       final double tabulatedTimeStep,
110                                                                                       final LOFType lofType,
111                                                                                       final TimeInterpolator<TimeStampedPair<Orbit, StateCovariance>> covarianceLOFInterpolator) {
112         // Given
113         final List<StateCovariance>                         referenceCovariances          = new ArrayList<>();
114         final List<StateCovariance>                         interpolatedCovariances       = new ArrayList<>();
115         final List<TimeStampedPair<Orbit, StateCovariance>> tabulatedOrbitsAndCovariances = new ArrayList<>();
116 
117         // Initialize reference state
118         final SpacecraftState sergeiState = generateSergeiReferenceState();
119         final Orbit           sergeiOrbit = sergeiState.getOrbit();
120         final AbsoluteDate    initialDate = sergeiOrbit.getDate();
121 
122         // Initialize reference covariance matrix
123         final RealMatrix sergeiCovarianceMatrix = generateSergeiCovarianceMatrix();
124 
125         // Initialize propagator
126         final NumericalPropagator propagator = new NumericalPropagator(
127                 generateDefaultIntegrator(sergeiOrbit, OrbitType.CARTESIAN));
128 
129         propagator.setOrbitType(OrbitType.CARTESIAN);
130 
131         // Initialize harvester
132         final MatricesHarvester harvester =
133                 propagator.setupMatricesComputation("harvester", null, null);
134 
135         // Initialize state covariance matrix provider
136         final StateCovariance sergeiCovariance =
137                 new StateCovariance(sergeiCovarianceMatrix, sergeiState.getDate(), sergeiState.getFrame(),
138                                     OrbitType.CARTESIAN, PositionAngleType.MEAN);
139 
140         final StateCovarianceMatrixProvider stateCovarianceMatrixProvider =
141                 new StateCovarianceMatrixProvider("covariance", "harvester", harvester, sergeiCovariance);
142 
143         // Configuring propagator
144         propagator.setInitialState(sergeiState);
145 
146         configurePropagatorForSergeiCase(propagator);
147 
148         propagator.addAdditionalDataProvider(stateCovarianceMatrixProvider);
149 
150         propagator.getMultiplexer().add(1, (currentState) -> {
151 
152             // Save reference covariance
153             final StateCovariance currentCovarianceFromProviderInCartesian =
154                     stateCovarianceMatrixProvider.getStateCovariance(currentState);
155 
156             // Convert to LOF
157             final StateCovariance currentCovarianceFromProviderInLOF =
158                     currentCovarianceFromProviderInCartesian.changeCovarianceFrame(currentState.getOrbit(), lofType);
159 
160             referenceCovariances.add(currentCovarianceFromProviderInLOF);
161 
162             // Save tabulated orbit and covariance
163             final double durationFromStart = currentState.getDate().durationFrom(sergeiState.getDate());
164             if (durationFromStart % tabulatedTimeStep == 0) {
165                 tabulatedOrbitsAndCovariances.add(new TimeStampedPair<>(currentState.getOrbit(),
166                                                                         currentCovarianceFromProviderInCartesian));
167             }
168         });
169 
170         // Propagation
171         propagator.propagate(initialDate.shiftedBy(propagationDuration));
172 
173         // Interpolate
174         for (int dt = 0; dt < referenceCovariances.size(); dt++) {
175             final AbsoluteDate currentInterpolationDate = initialDate.shiftedBy(dt);
176             final StateCovariance currentInterpolatedCovariance =
177                     covarianceLOFInterpolator.interpolate(currentInterpolationDate, tabulatedOrbitsAndCovariances)
178                                              .getSecond();
179 
180             interpolatedCovariances.add(currentInterpolatedCovariance);
181         }
182 
183         // Make statistics
184         return computeRMSRelativeErrorsStatistics(referenceCovariances,
185                                                   interpolatedCovariances);
186 
187     }
188 
189     /**
190      * Test given covariance interpolator on full force model test case from "TANYGIN, Sergei. Efficient covariance
191      * interpolation using blending of approximate covariance propagations. The Journal of the Astronautical Sciences, 2014,
192      * vol. 61, no 1, p. 107-132.".
193      * <p>
194      * For testing efficiency purpose, the propagation time shall be reduced instead of propagating throughout 2 hours.
195      *
196      * @param propagationDuration propagation duration (< 7200s)
197      * @param tabulatedTimeStep propagation time step
198      * @param covarianceInterpolator covariance interpolator to test
199      *
200      * @return statistics on relative position and velocity sigmas error throughout the interpolation.
201      */
202     public static DescriptiveStatistics[] computeStatisticsCovarianceInterpolationOnSergeiCase(
203             final double propagationDuration,
204             final double tabulatedTimeStep,
205             final TimeInterpolator<SpacecraftState> stateInterpolator,
206             final TimeInterpolator<TimeStampedPair<Orbit, StateCovariance>> covarianceInterpolator) {
207 
208         // Given
209         final List<StateCovariance> referenceCovariances    = new ArrayList<>();
210         final List<StateCovariance> interpolatedCovariances = new ArrayList<>();
211         final List<SpacecraftState> tabulatedStates         = new ArrayList<>();
212         final List<StateCovariance> tabulatedCovariances    = new ArrayList<>();
213 
214         // Initialize reference state
215         final SpacecraftState sergeiState = generateSergeiReferenceState();
216         final Orbit           sergeiOrbit = sergeiState.getOrbit();
217         final AbsoluteDate    initialDate = sergeiOrbit.getDate();
218 
219         // Initialize reference covariance matrix
220         final RealMatrix sergeiCovarianceMatrix = generateSergeiCovarianceMatrix();
221 
222         // Initialize propagator
223         final NumericalPropagator propagator = new NumericalPropagator(
224                 generateDefaultIntegrator(sergeiOrbit, OrbitType.CARTESIAN));
225 
226         propagator.setOrbitType(OrbitType.CARTESIAN);
227 
228         // Initialize harvester
229         final MatricesHarvester harvester =
230                 propagator.setupMatricesComputation("harvester", null, null);
231 
232         // Initialize state covariance matrix provider
233         final StateCovariance sergeiCovariance =
234                 new StateCovariance(sergeiCovarianceMatrix, sergeiState.getDate(), sergeiState.getFrame(),
235                                     OrbitType.CARTESIAN, PositionAngleType.MEAN);
236 
237         final StateCovarianceMatrixProvider stateCovarianceMatrixProvider =
238                 new StateCovarianceMatrixProvider("covariance", "harvester", harvester, sergeiCovariance);
239 
240         // Configuring propagator
241         propagator.setInitialState(sergeiState);
242 
243         configurePropagatorForSergeiCase(propagator);
244 
245         propagator.addAdditionalDataProvider(stateCovarianceMatrixProvider);
246 
247         propagator.getMultiplexer().add(1, (currentState) -> {
248 
249             // Save reference covariance
250             final StateCovariance currentCovarianceFromProviderInCartesian =
251                     stateCovarianceMatrixProvider.getStateCovariance(currentState);
252 
253             referenceCovariances.add(currentCovarianceFromProviderInCartesian);
254 
255             // Save tabulated state and covariance
256             final double durationFromStart = currentState.getDate().durationFrom(sergeiState.getDate());
257             if (durationFromStart % tabulatedTimeStep == 0) {
258                 tabulatedStates.add(currentState);
259                 tabulatedCovariances.add(currentCovarianceFromProviderInCartesian);
260             }
261         });
262 
263         // Propagation
264         propagator.propagate(initialDate.shiftedBy(propagationDuration));
265 
266         // Create custom Ephemeris
267         final Ephemeris ephemeris =
268                 new Ephemeris(tabulatedStates, stateInterpolator, tabulatedCovariances, covarianceInterpolator);
269 
270         // Interpolate
271         for (int dt = 0; dt < referenceCovariances.size(); dt++) {
272             final AbsoluteDate currentInterpolationDate = initialDate.shiftedBy(dt);
273 
274             interpolatedCovariances.add(ephemeris.getCovariance(currentInterpolationDate).get());
275         }
276 
277         // Make statistics
278         return computeRMSRelativeErrorsStatistics(referenceCovariances,
279                                                   interpolatedCovariances);
280 
281     }
282 
283     /**
284      * Compute statistics about RMS of relative error on position and velocity sigmas.
285      *
286      * @param referenceCovariances reference covariances
287      * @param interpolatedCovariances interpolated covariances
288      *
289      * @return statistics about RMS of relative error on position and velocity sigmas
290      */
291     public static DescriptiveStatistics[] computeRMSRelativeErrorsStatistics(
292             final List<StateCovariance> referenceCovariances,
293             final List<StateCovariance> interpolatedCovariances) {
294         final DescriptiveStatistics[] maxRelativeRMSPosAndVelError = new DescriptiveStatistics[2];
295 
296         final DescriptiveStatistics relativeRMSPosSigmaErrorStat = new DescriptiveStatistics();
297         final DescriptiveStatistics relativeRMSVelSigmaErrorStat = new DescriptiveStatistics();
298 
299         for (int i = 0; i < referenceCovariances.size(); i++) {
300 
301             final RealMatrix currentReferenceCovariance    = referenceCovariances.get(i).getMatrix();
302             final RealMatrix currentInterpolatedCovariance = interpolatedCovariances.get(i).getMatrix();
303 
304             final double[] currentReferenceSigmas    = extractSigmas(currentReferenceCovariance);
305             final double[] currentInterpolatedSigmas = extractSigmas(currentInterpolatedCovariance);
306 
307             final double[] currentReferencePosAndVelSigmas = computeRMSSigmaArray(currentReferenceCovariance);
308 
309             final double[] deltaPositionSigmas = new double[] {
310                     currentReferenceSigmas[0] - currentInterpolatedSigmas[0],
311                     currentReferenceSigmas[1] - currentInterpolatedSigmas[1],
312                     currentReferenceSigmas[2] - currentInterpolatedSigmas[2] };
313 
314             final double[] deltaVelocitySigmas = new double[] {
315                     currentReferenceSigmas[3] - currentInterpolatedSigmas[3],
316                     currentReferenceSigmas[4] - currentInterpolatedSigmas[4],
317                     currentReferenceSigmas[5] - currentInterpolatedSigmas[5] };
318 
319             // Add to statistics
320             final double relativeRMSPosSigmaError =
321                     computeRMS(deltaPositionSigmas) * 100 / currentReferencePosAndVelSigmas[0];
322             final double relativeRMSVelSigmaError =
323                     computeRMS(deltaVelocitySigmas) * 100 / currentReferencePosAndVelSigmas[1];
324 
325             relativeRMSPosSigmaErrorStat.addValue(relativeRMSPosSigmaError);
326             relativeRMSVelSigmaErrorStat.addValue(relativeRMSVelSigmaError);
327         }
328         maxRelativeRMSPosAndVelError[0] = relativeRMSPosSigmaErrorStat;
329         maxRelativeRMSPosAndVelError[1] = relativeRMSVelSigmaErrorStat;
330 
331         return maxRelativeRMSPosAndVelError;
332     }
333 
334     public static double[] computeRMSSigmaArray(final RealMatrix covarianceMatrix) {
335 
336         // RMS sigma for position and velocity
337         final double[] rmsSigmaArray = new double[2];
338 
339         final double[] positionSigmas = new double[] {
340                 FastMath.sqrt(FastMath.abs(covarianceMatrix.getEntry(0, 0))),
341                 FastMath.sqrt(FastMath.abs(covarianceMatrix.getEntry(1, 1))),
342                 FastMath.sqrt(FastMath.abs(covarianceMatrix.getEntry(2, 2))) };
343 
344         final double[] velocitySigmas = new double[] {
345                 FastMath.sqrt(FastMath.abs(covarianceMatrix.getEntry(3, 3))),
346                 FastMath.sqrt(FastMath.abs(covarianceMatrix.getEntry(4, 4))),
347                 FastMath.sqrt(FastMath.abs(covarianceMatrix.getEntry(5, 5))) };
348 
349         rmsSigmaArray[0] = computeRMS(positionSigmas);
350         rmsSigmaArray[1] = computeRMS(velocitySigmas);
351 
352         return rmsSigmaArray;
353     }
354 
355     /**
356      * Compute RMS of given data array.
357      *
358      * @param data data array
359      *
360      * @return RMS of given data array
361      */
362     public static double computeRMS(final double[] data) {
363         double sum = 0;
364         for (final double element : data) {
365             sum += element * element;
366         }
367         return FastMath.sqrt(sum / data.length);
368     }
369 
370     /**
371      * Extracts diagonal sigmas from given covariance 6x6 matrix.
372      *
373      * @param covarianceMatrix 6x6 covariance matrix
374      *
375      * @return diagonal sigmas
376      */
377     public static double[] extractSigmas(final RealMatrix covarianceMatrix) {
378         return new double[]
379                 { FastMath.sqrt(FastMath.abs(covarianceMatrix.getEntry(0, 0))),
380                   FastMath.sqrt(FastMath.abs(covarianceMatrix.getEntry(1, 1))),
381                   FastMath.sqrt(FastMath.abs(covarianceMatrix.getEntry(2, 2))),
382                   FastMath.sqrt(FastMath.abs(covarianceMatrix.getEntry(3, 3))),
383                   FastMath.sqrt(FastMath.abs(covarianceMatrix.getEntry(4, 4))),
384                   FastMath.sqrt(FastMath.abs(covarianceMatrix.getEntry(5, 5))) };
385     }
386 
387     public static ODEIntegrator generateDefaultIntegrator(final Orbit orbit, final OrbitType orbitType) {
388         final double     dP         = 1;
389         final double[][] tolerances = ToleranceProvider.getDefaultToleranceProvider(dP).getTolerances(orbit, orbitType);
390         final double     minStep    = 0.001;
391         final double     maxStep    = 300.;
392 
393         return new DormandPrince853Integrator(minStep, maxStep, tolerances[0], tolerances[1]);
394     }
395 
396     public static void configurePropagatorForSergeiCase(final NumericalPropagator propagator) {
397 
398         // Initialization
399         final IERSConventions conventions = IERSConventions.IERS_2010;
400         final Frame           itrf        = FramesFactory.getITRF(conventions, true);
401         final BodyShape earthShape = new OneAxisEllipsoid(Constants.IERS2010_EARTH_EQUATORIAL_RADIUS,
402                                                           Constants.IERS2010_EARTH_FLATTENING, itrf);
403 
404         final CelestialBody sun  = CelestialBodyFactory.getSun();
405         final CelestialBody moon = CelestialBodyFactory.getMoon();
406 
407         // Gravity fields
408         final NormalizedSphericalHarmonicsProvider gravity =
409                 GravityFieldFactory.getNormalizedProvider(21, 21);
410         propagator.addForceModel(new HolmesFeatherstoneAttractionModel(itrf, gravity));
411 
412         // Object dependant forces
413         final double crossSection = 20; // In m2
414 
415         // Solar radiation pressure (including Earth albedo)
416         final double             cr                 = 1;
417         final RadiationSensitive radiationSensitive = new IsotropicRadiationSingleCoefficient(crossSection, cr);
418         final SolarRadiationPressure solarRadiationPressure =
419                 new SolarRadiationPressure(sun,
420                                            ReferenceEllipsoid.getIers2010(itrf),
421                                            radiationSensitive);
422 
423         // Earth is already considered as an occulting body
424         solarRadiationPressure.addOccultingBody(moon, Constants.MOON_EQUATORIAL_RADIUS);
425 
426         final KnockeRediffusedForceModel albedo =
427                 new KnockeRediffusedForceModel(sun, radiationSensitive,
428                                                Constants.IERS2010_EARTH_EQUATORIAL_RADIUS,
429                                                FastMath.toRadians(5));
430 
431         propagator.addForceModel(solarRadiationPressure);
432         propagator.addForceModel(albedo);
433 
434         // Drag
435         final double        cd            = 2.2;
436         final IsotropicDrag dragSensitive = new IsotropicDrag(crossSection, cd);
437         final Atmosphere atmosphere =
438                 new NRLMSISE00(new CssiSpaceWeatherData(CssiSpaceWeatherData.DEFAULT_SUPPORTED_NAMES), sun, earthShape);
439         final DragForce dragForce = new DragForce(atmosphere, dragSensitive, false);
440         propagator.addForceModel(dragForce);
441 
442         // Third body
443         final ThirdBodyAttraction moonAttraction = new ThirdBodyAttraction(moon);
444         final ThirdBodyAttraction sunAttraction  = new ThirdBodyAttraction(sun);
445 
446         propagator.addForceModel(moonAttraction);
447         propagator.addForceModel(sunAttraction);
448 
449         // Solid tides
450         final SolidTides solidTides = new SolidTides(earthShape.getBodyFrame(), gravity.getAe(), gravity.getMu(),
451                                                      gravity.getTideSystem(), conventions,
452                                                      TimeScalesFactory.getUT1(conventions, true),
453                                                      moon, sun);
454         propagator.addForceModel(solidTides);
455 
456         // Ocean tides
457         final OceanTides oceanTides = new OceanTides(earthShape.getBodyFrame(), gravity.getAe(), gravity.getMu(),
458                                                      4, 4, conventions,
459                                                      TimeScalesFactory.getUT1(conventions, true));
460         propagator.addForceModel(oceanTides);
461 
462         // Relativity correction
463         final Relativity relativity = new Relativity(Constants.IERS2010_EARTH_MU);
464         propagator.addForceModel(relativity);
465 
466     }
467 
468     public static SpacecraftState generateSergeiReferenceState() {
469         final Orbit  orbit      = generateSergeiReferenceOrbit();
470         final double sergeiMass = 0.04;
471         return new SpacecraftState(orbit, sergeiMass);
472     }
473 
474     public static Orbit generateSergeiReferenceOrbit() {
475         // Initial sergei date that was modified in order to use the JPL ephemerides stored in the resources
476         //final AbsoluteDate sergeiDate = new AbsoluteDate(2008, 11, 22, 19, 0, 0, TimeScalesFactory.getUTC());
477         final AbsoluteDate sergeiDate = new AbsoluteDate(2003, 11, 22, 19, 0, 0, TimeScalesFactory.getUTC());
478 
479         return new CartesianOrbit(new PVCoordinates(
480                 new Vector3D(-2397200, 4217850, 5317450),
481                 new Vector3D(-1303.9, 5558.9, -4839.6)), FramesFactory.getGCRF(), sergeiDate, Constants.EIGEN5C_EARTH_MU);
482     }
483 
484     public static RealMatrix generateSergeiCovarianceMatrix() {
485 
486         // Sigma in position (in m)
487         final double sigmaPosX = 98676;
488         final double sigmaPosY = 420547;
489         final double sigmaPosZ = 366438;
490 
491         // Sigma in velocity (in m/s)
492         final double sigmaVelX = 194;
493         final double sigmaVelY = 341;
494         final double sigmaVelZ = 430;
495 
496         // Correlation in position
497         final double posXYCorr = -0.999985;
498         final double posXZCorr = 0.999983;
499         final double posYZCorr = -0.999997;
500 
501         // Correlation in velocity
502         final double velXYCorr = -0.999998;
503         final double velXZCorr = -0.999997;
504         final double velYZCorr = 0.999997;
505 
506         // Cross correlations
507         final double posXVelXCorr = -0.999982;
508         final double posXVelYCorr = 0.999989;
509         final double posXVelZCorr = 0.999983;
510 
511         final double posYVelXCorr = 0.999997;
512         final double posYVelYCorr = -0.999999;
513         final double posYVelZCorr = -0.999995;
514 
515         final double posZVelXCorr = -0.999996;
516         final double posZVelYCorr = 0.999996;
517         final double posZVelZCorr = 0.999999;
518 
519         return new BlockRealMatrix(new double[][] {
520                 { sigmaPosX * sigmaPosX, sigmaPosX * sigmaPosY * posXYCorr, sigmaPosX * sigmaPosZ * posXZCorr,
521                   sigmaPosX * sigmaVelX * posXVelXCorr, sigmaPosX * sigmaVelY * posXVelYCorr,
522                   sigmaPosX * sigmaVelZ * posXVelZCorr },
523                 { sigmaPosX * sigmaPosY * posXYCorr, sigmaPosY * sigmaPosY, sigmaPosY * sigmaPosZ * posYZCorr,
524                   sigmaPosY * sigmaVelX * posYVelXCorr, sigmaPosY * sigmaVelY * posYVelYCorr,
525                   sigmaPosY * sigmaVelZ * posYVelZCorr },
526                 { sigmaPosX * sigmaPosZ * posXZCorr, sigmaPosY * sigmaPosZ * posYZCorr, sigmaPosZ * sigmaPosZ,
527                   sigmaPosZ * sigmaVelX * posZVelXCorr, sigmaPosZ * sigmaVelY * posZVelYCorr,
528                   sigmaPosZ * sigmaVelZ * posZVelZCorr },
529                 { sigmaPosX * sigmaVelX * posXVelXCorr, sigmaPosY * sigmaVelX * posYVelXCorr,
530                   sigmaPosZ * sigmaVelX * posZVelXCorr, sigmaVelX * sigmaVelX, sigmaVelX * sigmaVelY * velXYCorr,
531                   sigmaVelX * sigmaVelZ * velXZCorr },
532                 { sigmaPosX * sigmaVelY * posXVelYCorr, sigmaPosY * sigmaVelY * posYVelYCorr,
533                   sigmaPosZ * sigmaVelY * posZVelYCorr, sigmaVelX * sigmaVelY * velXYCorr, sigmaVelY * sigmaVelY,
534                   sigmaVelY * sigmaVelZ * velYZCorr },
535                 { sigmaPosX * sigmaVelZ * posXVelZCorr, sigmaPosY * sigmaVelZ * posYVelZCorr,
536                   sigmaPosZ * sigmaVelZ * posZVelZCorr, sigmaVelX * sigmaVelZ * velXZCorr,
537                   sigmaVelY * sigmaVelZ * velYZCorr, sigmaVelZ * sigmaVelZ } });
538     }
539 
540     private void doTestInterpolation(final TimeInterpolator<SpacecraftState> stateInterpolator,
541                                      final TimeInterpolator<TimeStampedPair<Orbit, StateCovariance>> covarianceInterpolator,
542                                      final double propagationHorizon,
543                                      final double tabulatedTimeStep,
544                                      final double expectedMeanRMSPositionError,
545                                      final double expectedMeanRMSVelocityError,
546                                      final double expectedMedianRMSPositionError,
547                                      final double expectedMedianRMSVelocityError,
548                                      final double expectedMaxRMSPositionError,
549                                      final double expectedMaxRMSVelocityError,
550                                      final double tolerance,
551                                      final boolean showResults) {
552         final DescriptiveStatistics[] relativeRMSSigmaError =
553                 computeStatisticsCovarianceInterpolationOnSergeiCase(propagationHorizon, tabulatedTimeStep,
554                                                                      stateInterpolator, covarianceInterpolator);
555 
556         // Then
557         if (showResults) {
558             // Significant number to display
559             final int nDigits = (int) FastMath.log10(1/tolerance);
560             final String fmt = String.format(Locale.US, "%%35s = %%%2d.%2df%%n", nDigits + 4, nDigits);
561             
562             System.out.format(Locale.US, fmt, "relativeRMSSigmaError[0].getMean", relativeRMSSigmaError[0].getMean());
563             System.out.format(Locale.US, fmt, "relativeRMSSigmaError[1].getMean", relativeRMSSigmaError[1].getMean());
564             System.out.format(Locale.US, fmt, "relativeRMSSigmaError[0].getMedian", relativeRMSSigmaError[0].getPercentile(50));
565             System.out.format(Locale.US, fmt, "relativeRMSSigmaError[1].getMedian", relativeRMSSigmaError[1].getPercentile(50));
566             System.out.format(Locale.US, fmt, "relativeRMSSigmaError[0].getMax", relativeRMSSigmaError[0].getMax());
567             System.out.format(Locale.US, fmt, "relativeRMSSigmaError[1].getMax", relativeRMSSigmaError[1].getMax());
568             
569         }
570         // Results obtained when using modified orbit date to use truncated JPL test resource file
571         Assertions.assertEquals(expectedMeanRMSPositionError, relativeRMSSigmaError[0].getMean(), tolerance);
572         Assertions.assertEquals(expectedMeanRMSVelocityError, relativeRMSSigmaError[1].getMean(), tolerance);
573         Assertions.assertEquals(expectedMedianRMSPositionError, relativeRMSSigmaError[0].getPercentile(50), tolerance);
574         Assertions.assertEquals(expectedMedianRMSVelocityError, relativeRMSSigmaError[1].getPercentile(50), tolerance);
575         Assertions.assertEquals(expectedMaxRMSPositionError, relativeRMSSigmaError[0].getMax(), tolerance);
576         Assertions.assertEquals(expectedMaxRMSVelocityError, relativeRMSSigmaError[1].getMax(), tolerance);
577     }
578 
579     /**
580      * Test based on the full force model test case from TANYGIN, Sergei. Efficient covariance interpolation using blending
581      * of approximate covariance propagations. The Journal of the Astronautical Sciences, 2014, vol. 61, no 1, p. 107-132.
582      * <p>
583      * However, note that the exact result is not known and only an approximated value can be deduced from the available
584      * graph in the aforementioned paper. Although the value obtained using a quintic Hermite interpolator is different from
585      * values found in the paper, no errors were found in the algorithm. Furthermore, the results obtained in the paper are
586      * not thoroughly explained so the exact cause is unknown.
587      * <p>
588      * This instance of the test is a non regression test aiming to test the results obtained when using both Keplerian time
589      * derivatives. Hence, a quintic Keplerian interpolation.
590      */
591     @Test
592     @DisplayName("test quintic Keplerian interpolation (two time derivatives used) on full force model test case from : "
593             + "TANYGIN, Sergei. Efficient covariance interpolation using blending of approximate covariance propagations. "
594             + "The Journal of the Astronautical Sciences, 2014, vol. 61, no 1, p. 107-132.")
595     void testQuinticKeplerianInterpolation() {
596 
597         // Given
598         final boolean showResults = false; // Show results?
599         final double tolerance = 1.e-9;
600         
601         // Create state covariance interpolator
602         final SmoothStepFactory.SmoothStepFunction blendingFunction = SmoothStepFactory.getQuadratic();
603 
604         final TimeInterpolator<Orbit> orbitInterpolator =
605                 new OrbitBlender(blendingFunction, new KeplerianPropagator(sergeiOrbit), sergeiFrame);
606 
607         final StateCovarianceKeplerianHermiteInterpolator covarianceInterpolator =
608                 new StateCovarianceKeplerianHermiteInterpolator(orbitInterpolator, sergeiFrame, OrbitType.CARTESIAN,
609                                                                 PositionAngleType.MEAN);
610 
611         // Create state interpolator
612         final TimeInterpolator<SpacecraftState> stateInterpolator =
613                 new SpacecraftStateInterpolator(AbstractTimeInterpolator.DEFAULT_INTERPOLATION_POINTS, AbstractTimeInterpolator.DEFAULT_EXTRAPOLATION_THRESHOLD_SEC,
614                                                 sergeiFrame, orbitInterpolator, 
615                                                 null, null, null, null);
616 
617         // When & Then
618         doTestInterpolation(stateInterpolator, covarianceInterpolator,
619                             DEFAULT_SERGEI_PROPAGATION_TIME, DEFAULT_SERGEI_TABULATED_TIMESTEP,
620                             0.06468878849,
621                             0.18700110863,
622                             0.06052527006,
623                             0.20900926133,
624                             0.17225595779,
625                             0.37560104200,
626                             tolerance,
627                             showResults);
628 
629 /*      Results obtained when using the reference sergei date
630         Assertions.assertEquals(0.08333354122902344, relativeRMSSigmaError[0].getMean(), 1e-17);
631         Assertions.assertEquals(0.18339504723198177, relativeRMSSigmaError[1].getMean(), 1e-17);
632         Assertions.assertEquals(0.08379904791529535, relativeRMSSigmaError[0].getPercentile(50), 1e-17);
633         Assertions.assertEquals(0.21301699586775608, relativeRMSSigmaError[1].getPercentile(50), 1e-17);
634         Assertions.assertEquals(0.18097897860458778, relativeRMSSigmaError[0].getMax(), 1e-17);
635         Assertions.assertEquals(0.25871013837895007, relativeRMSSigmaError[1].getMax(), 1e-17);
636 */
637 
638         Assertions.assertEquals(CartesianDerivativesFilter.USE_PVA, covarianceInterpolator.getFilter());
639 
640     }
641 
642     /**
643      * Test based on the full force model test case from TANYGIN, Sergei. Efficient covariance interpolation using blending
644      * of approximate covariance propagations. The Journal of the Astronautical Sciences, 2014, vol. 61, no 1, p. 107-132.
645      * <p>
646      * This instance of the test is a non regression test aiming to test the results obtained when using the first Keplerian
647      * time derivative only. Hence, a cubic Keplerian interpolation.
648      */
649     @Test
650     @DisplayName("test cubic Keplerian interpolation (first time derivative used) on full force model test case from : "
651             + "TANYGIN, Sergei. Efficient covariance interpolation using blending of approximate covariance propagations. "
652             + "The Journal of the Astronautical Sciences, 2014, vol. 61, no 1, p. 107-132.")
653     void testCubicKeplerianInterpolation() {
654 
655         // Given
656         final boolean showResults = false; // Show results?
657         final double tolerance = 1.e-9;
658         
659         // Create state covariance interpolator
660         final SmoothStepFactory.SmoothStepFunction blendingFunction = SmoothStepFactory.getQuadratic();
661 
662         final TimeInterpolator<Orbit> orbitInterpolator = new OrbitBlender(blendingFunction,
663                                                                            new KeplerianPropagator(sergeiOrbit),
664                                                                            sergeiFrame);
665 
666         final StateCovarianceKeplerianHermiteInterpolator covarianceInterpolator =
667                 new StateCovarianceKeplerianHermiteInterpolator(DEFAULT_SERGEI_INTERPOLATION_POINTS, orbitInterpolator,
668                                                                 CartesianDerivativesFilter.USE_PV, sergeiFrame,
669                                                                 OrbitType.CARTESIAN,
670                                                                 PositionAngleType.MEAN);
671 
672         // Create state interpolator
673         final TimeInterpolator<SpacecraftState> stateInterpolator =
674                 new SpacecraftStateInterpolator(AbstractTimeInterpolator.DEFAULT_INTERPOLATION_POINTS, AbstractTimeInterpolator.DEFAULT_EXTRAPOLATION_THRESHOLD_SEC,
675                                                 sergeiFrame, orbitInterpolator, null, null, null, null);
676 
677         // When & then
678         doTestInterpolation(stateInterpolator, covarianceInterpolator,
679                             DEFAULT_SERGEI_PROPAGATION_TIME, DEFAULT_SERGEI_TABULATED_TIMESTEP,
680                             0.0687107434,
681                             0.1727658634,
682                             0.06966854473,
683                             0.17880639263,
684                             0.17028706967,
685                             0.3841670162,
686                             tolerance,
687                             showResults);
688 
689         // Results obtained when using Sergei reference date
690 /*        Assertions.assertEquals(0.07740033278409426, relativeRMSSigmaError[0].getMean(), 1e-17);
691         Assertions.assertEquals(0.16752174969304912, relativeRMSSigmaError[1].getMean(), 1e-17);
692         Assertions.assertEquals(0.08063527083126852, relativeRMSSigmaError[0].getPercentile(50), 1e-17);
693         Assertions.assertEquals(0.1926905326066871 , relativeRMSSigmaError[1].getPercentile(50), 1e-17);
694         Assertions.assertEquals(0.16289839792811542, relativeRMSSigmaError[0].getMax(), 1e-17);
695         Assertions.assertEquals(0.23616924578204512, relativeRMSSigmaError[1].getMax(), 1e-17);*/
696 
697         Assertions.assertEquals(CartesianDerivativesFilter.USE_PV, covarianceInterpolator.getFilter());
698 
699     }
700 
701     /**
702      * Test based on the full force model test case from TANYGIN, Sergei. Efficient covariance interpolation using blending
703      * of approximate covariance propagations. The Journal of the Astronautical Sciences, 2014, vol. 61, no 1, p. 107-132.
704      * <p>
705      * This instance of the test is a non regression test aiming to test the results obtained when not using Keplerian time
706      * derivatives. Hence, a linear Keplerian interpolation.
707      */
708     @Test
709     @DisplayName("test linear Keplerian interpolation (no derivatives used) on full force model test case from : "
710             + "TANYGIN, Sergei. Efficient covariance interpolation using blending of approximate covariance propagations. "
711             + "The Journal of the Astronautical Sciences, 2014, vol. 61, no 1, p. 107-132.")
712     void testLinearKeplerianInterpolation() {
713 
714         // Given
715         final boolean showResults = false; // Show results?
716         final double tolerance = 1.e-9;
717         
718         // Create state covariance interpolator
719         final SmoothStepFactory.SmoothStepFunction blendingFunction = SmoothStepFactory.getQuadratic();
720 
721         final TimeInterpolator<Orbit> orbitInterpolator = new OrbitBlender(blendingFunction,
722                                                                            new KeplerianPropagator(sergeiOrbit),
723                                                                            sergeiFrame);
724 
725         final StateCovarianceKeplerianHermiteInterpolator covarianceInterpolator =
726                 new StateCovarianceKeplerianHermiteInterpolator(DEFAULT_SERGEI_INTERPOLATION_POINTS, orbitInterpolator,
727                                                                 CartesianDerivativesFilter.USE_P, sergeiFrame,
728                                                                 OrbitType.CARTESIAN,
729                                                                 PositionAngleType.MEAN);
730 
731         // Create state interpolator
732         final TimeInterpolator<SpacecraftState> stateInterpolator =
733                 new SpacecraftStateInterpolator(AbstractTimeInterpolator.DEFAULT_INTERPOLATION_POINTS, AbstractTimeInterpolator.DEFAULT_EXTRAPOLATION_THRESHOLD_SEC,
734                                                 sergeiFrame, orbitInterpolator, null, null, null, null);
735 
736         // When & Then
737         doTestInterpolation(stateInterpolator, covarianceInterpolator,
738                             DEFAULT_SERGEI_PROPAGATION_TIME, DEFAULT_SERGEI_TABULATED_TIMESTEP,
739                             0.1967532898,
740                             0.174480997,
741                             0.220130098,
742                             0.150103817,
743                             0.311560976,
744                             0.491299092,
745                             tolerance, 
746                             showResults);
747 
748         // Results obtained when using Sergei reference date
749 /*        Assertions.assertEquals(0.09148580146577297, relativeRMSSigmaError[0].getMean(), 1e-17);
750         Assertions.assertEquals(0.11704748448308232, relativeRMSSigmaError[1].getMean(), 1e-17);
751         Assertions.assertEquals(0.09727415341226611, relativeRMSSigmaError[0].getPercentile(50), 1e-17);
752         Assertions.assertEquals(0.12457112100482712, relativeRMSSigmaError[1].getPercentile(50), 1e-17);
753         Assertions.assertEquals(0.16611131341788263, relativeRMSSigmaError[0].getMax(), 1e-17);
754         Assertions.assertEquals(0.1922012892962485, relativeRMSSigmaError[1].getMax(), 1e-17);*/
755 
756         Assertions.assertEquals(CartesianDerivativesFilter.USE_P, covarianceInterpolator.getFilter());
757 
758     }
759 
760     /**
761      * Test based on the full force model test case from TANYGIN, Sergei. Efficient covariance interpolation using blending
762      * of approximate covariance propagations. The Journal of the Astronautical Sciences, 2014, vol. 61, no 1, p. 107-132.
763      * <p>
764      * This instance of the test is a non regression test aiming to test the results obtained when interpolating in a local
765      * orbital frame.
766      * <p>
767      * Bad results are seen regarding velocity interpolation. This has been checked to be independent of the method used to
768      * interpolate the state covariance. Moreover, the same results are achieved if we interpolate in an inertial frame
769      * (which has been seen to give very good results as in {@link #testQuinticKeplerianInterpolation()}),and then express it
770      * in a non-inertial local orbital frame. It has also been verified that it was not (mainly) due to errors in orbit
771      * interpolation as even using exact orbit for frame conversion would only slightly improve the results (<0.1%
772      * improvements). Hence, the only explanation found is a sensitivity issue linked to non-inertial local orbital frame.
773      */
774     @Test
775     @DisplayName("test quintic Keplerian interpolation (output in LOF) on full force model test case from : "
776             + "TANYGIN, Sergei. Efficient covariance interpolation using blending of approximate covariance propagations. "
777             + "The Journal of the Astronautical Sciences, 2014, vol. 61, no 1, p. 107-132.")
778     void testLOFInterpolation() {
779 
780         // Given
781         final boolean showResults = false; // Show results?
782         final double tolerance = 1.e-9;
783         
784         // Default orbit case
785         final Orbit orbit = generateSergeiReferenceOrbit();
786         final Frame frame = orbit.getFrame();
787 
788         // Create state covariance interpolator
789         final SmoothStepFactory.SmoothStepFunction blendingFunction = SmoothStepFactory.getQuadratic();
790 
791         final TimeInterpolator<Orbit> orbitInterpolator = new OrbitBlender(blendingFunction,
792                                                                            new KeplerianPropagator(orbit),
793                                                                            frame);
794 
795         final LOFType DEFAULT_LOFTYPE = LOFType.TNW;
796         final TimeInterpolator<TimeStampedPair<Orbit, StateCovariance>> covarianceInterpolator =
797                 new StateCovarianceKeplerianHermiteInterpolator(orbitInterpolator, DEFAULT_LOFTYPE);
798 
799         // When
800         final DescriptiveStatistics[] relativeRMSSigmaError =
801                 computeStatisticsCovarianceLOFInterpolation(DEFAULT_SERGEI_PROPAGATION_TIME,
802                                                             DEFAULT_SERGEI_TABULATED_TIMESTEP,
803                                                             DEFAULT_LOFTYPE, covarianceInterpolator);
804 
805         // Then
806         if (showResults) {
807             System.out.format(Locale.US, "%35s = %20.12f%n", "relativeRMSSigmaError[0].getMean", relativeRMSSigmaError[0].getMean());
808             System.out.format(Locale.US, "%35s = %20.12f%n", "relativeRMSSigmaError[1].getMean", relativeRMSSigmaError[1].getMean());
809             System.out.format(Locale.US, "%35s = %20.12f%n", "relativeRMSSigmaError[0].getMedian", relativeRMSSigmaError[0].getPercentile(50));
810             System.out.format(Locale.US, "%35s = %20.12f%n", "relativeRMSSigmaError[1].getMedian", relativeRMSSigmaError[1].getPercentile(50));
811             System.out.format(Locale.US, "%35s = %20.12f%n", "relativeRMSSigmaError[0].getMax", relativeRMSSigmaError[0].getMax());
812             System.out.format(Locale.US, "%35s = %20.12f%n", "relativeRMSSigmaError[1].getMax", relativeRMSSigmaError[1].getMax());
813             
814         }
815         Assertions.assertEquals( 0.067889396, relativeRMSSigmaError[0].getMean(), tolerance);
816         Assertions.assertEquals( 20.113671554, relativeRMSSigmaError[1].getMean(), tolerance);
817         Assertions.assertEquals( 0.064925239, relativeRMSSigmaError[0].getPercentile(50), tolerance);
818         Assertions.assertEquals( 13.962696065, relativeRMSSigmaError[1].getPercentile(50), tolerance);
819         Assertions.assertEquals( 0.140595553, relativeRMSSigmaError[0].getMax(), tolerance);
820         Assertions.assertEquals(99.8740338063, relativeRMSSigmaError[1].getMax(), tolerance);
821     }
822 }