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.ssa.collision.shorttermencounter.probability.twod;
18  
19  import org.hipparchus.Field;
20  import org.hipparchus.analysis.differentiation.DSFactory;
21  import org.hipparchus.analysis.differentiation.DerivativeStructure;
22  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
23  import org.hipparchus.geometry.euclidean.threed.Vector3D;
24  import org.hipparchus.linear.BlockFieldMatrix;
25  import org.hipparchus.linear.BlockRealMatrix;
26  import org.hipparchus.linear.FieldMatrix;
27  import org.hipparchus.linear.RealMatrix;
28  import org.hipparchus.stat.descriptive.DescriptiveStatistics;
29  import org.hipparchus.util.Binary64;
30  import org.hipparchus.util.Binary64Field;
31  import org.junit.jupiter.api.Assertions;
32  import org.junit.jupiter.api.BeforeAll;
33  import org.junit.jupiter.api.DisplayName;
34  import org.junit.jupiter.api.Test;
35  import org.orekit.Utils;
36  import org.orekit.frames.Frame;
37  import org.orekit.frames.FramesFactory;
38  import org.orekit.frames.LOFType;
39  import org.orekit.orbits.CartesianOrbit;
40  import org.orekit.orbits.FieldCartesianOrbit;
41  import org.orekit.orbits.FieldOrbit;
42  import org.orekit.orbits.Orbit;
43  import org.orekit.propagation.FieldStateCovariance;
44  import org.orekit.propagation.StateCovariance;
45  import org.orekit.ssa.collision.shorttermencounter.probability.twod.armellinutils.ArmellinDataLoader;
46  import org.orekit.ssa.collision.shorttermencounter.probability.twod.armellinutils.ArmellinDataRow;
47  import org.orekit.ssa.collision.shorttermencounter.probability.twod.armellinutils.ArmellinStatistics;
48  import org.orekit.ssa.metrics.FieldProbabilityOfCollision;
49  import org.orekit.ssa.metrics.ProbabilityOfCollision;
50  import org.orekit.time.AbsoluteDate;
51  import org.orekit.time.FieldAbsoluteDate;
52  import org.orekit.utils.Constants;
53  import org.orekit.utils.FieldPVCoordinates;
54  import org.orekit.utils.PVCoordinates;
55  
56  import java.io.IOException;
57  import java.util.List;
58  
59  class Alfriend1999MaxTest {
60  
61      /**
62       * Simple method to compute probability of collision assuming a constant density of probability of collision.
63       */
64      private final ShortTermEncounter2DPOCMethod method = new Alfriend1999Max();
65  
66      @BeforeAll
67      static void initializeOrekitData() {
68          Utils.setDataRoot("regular-data");
69      }
70  
71      /**
72       * This method use the data from the appendix (p.13) of "Armellin, R. (2021). Collision Avoidance Maneuver Optimization
73       * with a Multiple-Impulse Convex Formulation."
74       */
75      @Test
76      @DisplayName("Test this method on Armellin appendix test case")
77      void testComputeExpectedMaximumProbabilityOfCollisionFromArmellinAppendix() {
78  
79          // GIVEN
80  
81          // Define the time of closest approach and mu
82          final AbsoluteDate timeOfClosestApproach = new AbsoluteDate();
83          final double       mu                    = Constants.IERS2010_EARTH_MU;
84  
85          // Value of combined radius from Armellin's paper appendix (m)
86          final double combinedRadius = 29.71;
87  
88          // Define the primary collision object
89          final Frame primaryInertialFrame = FramesFactory.getEME2000();
90          final Orbit primary = new CartesianOrbit(
91                  new PVCoordinates(new Vector3D(2.33052185175137e3, -1.10370451050201e6, 7.10588764299718e6),
92                                    new Vector3D(-7.44286282871773e3, -6.13734743652660e-1, 3.95136139293349e0)),
93                  primaryInertialFrame, timeOfClosestApproach, mu);
94          final RealMatrix primaryCovarianceMatrixInPrimaryRTN = new BlockRealMatrix(
95                  new double[][] { { 9.31700905887535e1, -2.623398113500550e2, 2.360382173935300e1, 0, 0, 0 },
96                                   { -2.623398113500550e2, 1.77796454279511e4, -9.331225387386501e1, 0, 0, 0 },
97                                   { 2.360382173935300e1, -9.331225387386501e1, 1.917372231880040e1, 0, 0, 0 },
98                                   { 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 } });
99          final StateCovariance primaryCovariance = new StateCovariance(primaryCovarianceMatrixInPrimaryRTN,
100                                                                       timeOfClosestApproach, LOFType.QSW);
101         final double primaryRadius = combinedRadius / 2;
102 
103         // Define the secondary collision object
104         final Frame secondaryInertialFrame = FramesFactory.getEME2000();
105         final Orbit secondary = new CartesianOrbit(
106                 new PVCoordinates(new Vector3D(2.333465506263321e3, -1.103671212478364e6, 7.105914958099038e6),
107                                   new Vector3D(7.353740487126315e3, -1.142814049765362e3, -1.982472259113771e2)),
108                 secondaryInertialFrame, timeOfClosestApproach, mu);
109         final RealMatrix secondaryCovarianceMatrixInSecondaryRTN = new BlockRealMatrix(
110                 new double[][] { { 6.346570910720371e2, -1.962292216245289e3, 7.077413655227660e1, 0, 0, 0 },
111                                  { -1.962292216245289e3, 8.199899363150306e5, 1.139823810584350e3, 0, 0, 0 },
112                                  { 7.077413655227660e1, 1.139823810584350e3, 2.510340829074070e2, 0, 0, 0 },
113                                  { 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 } });
114         final StateCovariance secondaryCovariance = new StateCovariance(secondaryCovarianceMatrixInSecondaryRTN,
115                                                                         timeOfClosestApproach, LOFType.QSW);
116         final double secondaryRadius = combinedRadius / 2;
117 
118         final ShortTermEncounter2DDefinition collision = new ShortTermEncounter2DDefinition(primary, primaryCovariance,
119                                                                                             primaryRadius, secondary,
120                                                                                             secondaryCovariance,
121                                                                                             secondaryRadius);
122 
123         // WHEN
124         final ProbabilityOfCollision result = method.compute(collision);
125 
126         // THEN
127         Assertions.assertEquals(1.9259e-1, result.getValue(), 1e-6);
128     }
129 
130     @Test
131     @DisplayName("Test this method on Armellin's data and compare statistics")
132     void testCompareStatisticsAboutMaximumProbabilityOfCollisionWithArmellinData() throws IOException {
133         // GIVEN
134         final List<ArmellinDataRow> armellinDataRowList = ArmellinDataLoader.load();
135 
136         // WHEN
137         final DescriptiveStatistics statistics = ArmellinStatistics.getMaxProbabilityOfCollisionRelativeDifferenceStatistics(
138                 armellinDataRowList);
139 
140         // THEN
141         Assertions.assertTrue(statistics.getMean() <= 2.4659494534898345E-10);
142         Assertions.assertTrue(statistics.getStandardDeviation() <= 6.702417431649529E-10);
143     }
144 
145     /**
146      * This method use the data from the appendix (p.13) of "Armellin, R. (2021). Collision Avoidance Maneuver Optimization
147      * with a Multiple-Impulse Convex Formulation."
148      */
149     @Test
150     @DisplayName("Test this method on Armellin appendix test case")
151     void testComputeExpectedMaximumProbabilityOfCollisionFromArmellinAppendixField() {
152 
153         // GIVEN
154 
155         // Define the time of closest approach and mu
156         final Field<Binary64>             field                 = Binary64Field.getInstance();
157         final FieldAbsoluteDate<Binary64> timeOfClosestApproach = new FieldAbsoluteDate<>(field);
158         final Binary64                    mu                    = new Binary64(Constants.IERS2010_EARTH_MU);
159 
160         // Value of combined radius from Armellin's paper appendix (m)
161         final Binary64 combinedRadius = new Binary64(29.71);
162 
163         // Define the primary collision object
164         final Frame primaryInertialFrame = FramesFactory.getEME2000();
165 
166         final FieldOrbit<Binary64> primary = new FieldCartesianOrbit<>(
167                 new FieldPVCoordinates<>(
168                         new FieldVector3D<>(new Binary64(2.33052185175137e3), new Binary64(-1.10370451050201e6),
169                                             new Binary64(7.10588764299718e6)),
170                         new FieldVector3D<>(new Binary64(-7.44286282871773e3), new Binary64(-6.13734743652660e-1),
171                                             new Binary64(3.95136139293349e0))),
172                 primaryInertialFrame, timeOfClosestApproach, mu);
173 
174         final FieldMatrix<Binary64> primaryCovarianceMatrixInPrimaryRTN = new BlockFieldMatrix<>(
175                 new Binary64[][] { { new Binary64(9.31700905887535e1), new Binary64(-2.623398113500550e2), new Binary64(
176                         2.360382173935300e1), new Binary64(0), new Binary64(0), new Binary64(0) },
177                                    { new Binary64(-2.623398113500550e2), new Binary64(1.77796454279511e4), new Binary64(
178                                            -9.331225387386501e1), new Binary64(0), new Binary64(0), new Binary64(0) },
179                                    { new Binary64(2.360382173935300e1), new Binary64(-9.331225387386501e1), new Binary64(
180                                            1.917372231880040e1), new Binary64(0), new Binary64(0), new Binary64(0) },
181                                    { new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(
182                                            0), new Binary64(0) }, { new Binary64(0), new Binary64(0), new Binary64(
183                         0), new Binary64(0), new Binary64(0), new Binary64(0) }, { new Binary64(0), new Binary64(
184                         0), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0) } });
185 
186         final FieldStateCovariance<Binary64> primaryCovariance =
187                 new FieldStateCovariance<>(primaryCovarianceMatrixInPrimaryRTN,
188                                            timeOfClosestApproach, LOFType.QSW_INERTIAL);
189 
190         final Binary64 primaryRadius = combinedRadius.multiply(0.5);
191 
192         // Define the secondary collision object
193         final Frame secondaryInertialFrame = FramesFactory.getEME2000();
194 
195         final FieldOrbit<Binary64> secondary = new FieldCartesianOrbit<>(
196                 new FieldPVCoordinates<>(
197                         new FieldVector3D<>(new Binary64(2.333465506263321e3), new Binary64(-1.103671212478364e6),
198                                             new Binary64(7.105914958099038e6)),
199                         new FieldVector3D<>(new Binary64(7.353740487126315e3), new Binary64(-1.142814049765362e3),
200                                             new Binary64(-1.982472259113771e2))),
201                 secondaryInertialFrame, timeOfClosestApproach, mu);
202 
203         final FieldMatrix<Binary64> secondaryCovarianceMatrixInSecondaryRTN = new BlockFieldMatrix<>(
204                 new Binary64[][] { { new Binary64(6.346570910720371e2), new Binary64(-1.962292216245289e3), new Binary64(
205                         7.077413655227660e1), new Binary64(0), new Binary64(0), new Binary64(0) },
206                                    { new Binary64(-1.962292216245289e3), new Binary64(8.199899363150306e5), new Binary64(
207                                            1.139823810584350e3), new Binary64(0), new Binary64(0), new Binary64(0) },
208                                    { new Binary64(7.077413655227660e1), new Binary64(1.139823810584350e3), new Binary64(
209                                            2.510340829074070e2), new Binary64(0), new Binary64(0), new Binary64(0) },
210                                    { new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(
211                                            0), new Binary64(0) }, { new Binary64(0), new Binary64(0), new Binary64(
212                         0), new Binary64(0), new Binary64(0), new Binary64(0) }, { new Binary64(0), new Binary64(
213                         0), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0) } });
214 
215         final FieldStateCovariance<Binary64> secondaryCovariance =
216                 new FieldStateCovariance<>(secondaryCovarianceMatrixInSecondaryRTN,
217                                            timeOfClosestApproach, LOFType.QSW_INERTIAL);
218 
219         final Binary64 secondaryRadius = combinedRadius.multiply(0.5);
220 
221         final FieldShortTermEncounter2DDefinition<Binary64> collision =
222                 new FieldShortTermEncounter2DDefinition<>(primary, primaryCovariance,
223                                                           primaryRadius, secondary,
224                                                           secondaryCovariance,
225                                                           secondaryRadius);
226 
227         // WHEN
228         final FieldProbabilityOfCollision<Binary64> result = method.compute(collision);
229 
230         // THEN
231         Assertions.assertEquals(1.9259e-1, result.getValue().getReal(), 1e-6);
232     }
233 
234     @Test
235     @DisplayName("Test this method on Armellin's data and compare statistics")
236     void testCompareStatisticsAboutMaximumProbabilityOfCollisionWithArmellinDataField() throws IOException {
237         // GIVEN
238         final List<ArmellinDataRow> armellinDataRowList = ArmellinDataLoader.load();
239 
240         // WHEN
241         final DescriptiveStatistics statistics =
242                 ArmellinStatistics.getMaxFieldProbabilityOfCollisionRelativeDifferenceStatistics(
243                         armellinDataRowList);
244 
245         // THEN
246         Assertions.assertTrue(statistics.getMean() <= 2.475695820847676E-10);
247         Assertions.assertTrue(statistics.getStandardDeviation() <= 6.748768612457567E-10);
248     }
249 
250     @Test
251     @DisplayName("Test this method on Armellin appendix test case with taylor approximation")
252     void testReturnExpectedValueWithDerivative() {
253         // GIVEN
254         final DSFactory factory = new DSFactory(5, 6);
255 
256         // Data extracted from other test
257         final double xmNominal     = 20.711983607206943;
258         final double ymNominal     = -37.87548026356126;
259         final double sigmaXNominal = 26.841611626440486;
260         final double sigmaYNominal = 72.06451864988387;
261         final double radiusNominal = 29.71;
262 
263         final double dxm     = 10;
264         final double dym     = 18;
265         final double dSigmaX = 13;
266         final double dSigmaY = 30;
267         final double dRadius = 15;
268 
269         final DerivativeStructure xm     = factory.variable(0, xmNominal);
270         final DerivativeStructure ym     = factory.variable(1, ymNominal);
271         final DerivativeStructure sigmaX = factory.variable(2, sigmaXNominal);
272         final DerivativeStructure sigmaY = factory.variable(3, sigmaYNominal);
273         final DerivativeStructure radius = factory.variable(4, radiusNominal);
274 
275         // WHEN
276         final FieldProbabilityOfCollision<DerivativeStructure> resultNominal =
277                 method.compute(xm, ym, sigmaX, sigmaY, radius);
278         final double taylorResult = resultNominal.getValue().taylor(dxm, dym, dSigmaX, dSigmaY, dRadius);
279         final double exactResult = method.compute(xmNominal + dxm, ymNominal + dym, sigmaXNominal + dSigmaX,
280                                                   sigmaYNominal + dSigmaY, radiusNominal + dRadius).getValue();
281 
282         // THEN
283         Assertions.assertEquals(0.19259, resultNominal.getValue().getReal(), 1e-6);
284         Assertions.assertEquals(exactResult, taylorResult, 1e-3);
285     }
286 }