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 Alfriend1999Test {
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 Alfriend1999();
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 testComputeExpectedApproximatedProbabilityOfCollisionFromArmellinAppendix() {
78  
79          // GIVEN
80          // Define the time of closest approach and mu
81          final AbsoluteDate timeOfClosestApproach = new AbsoluteDate();
82          final double       mu                    = Constants.IERS2010_EARTH_MU;
83  
84          // Value of combined radius from Armellin's paper appendix (m)
85          final double combinedRadius = 29.71;
86  
87          // Define the primary collision object
88          final Frame primaryInertialFrame = FramesFactory.getEME2000();
89          final Orbit primary = new CartesianOrbit(
90                  new PVCoordinates(new Vector3D(2.33052185175137e3, -1.10370451050201e6, 7.10588764299718e6),
91                                    new Vector3D(-7.44286282871773e3, -6.13734743652660e-1, 3.95136139293349e0)),
92                  primaryInertialFrame, timeOfClosestApproach, mu);
93          final RealMatrix primaryCovarianceMatrixInPrimaryRTN = new BlockRealMatrix(
94                  new double[][] { { 9.31700905887535e1, -2.623398113500550e2, 2.360382173935300e1, 0, 0, 0 },
95                                   { -2.623398113500550e2, 1.77796454279511e4, -9.331225387386501e1, 0, 0, 0 },
96                                   { 2.360382173935300e1, -9.331225387386501e1, 1.917372231880040e1, 0, 0, 0 },
97                                   { 0, 0, 0, 0, 0, 0 },
98                                   { 0, 0, 0, 0, 0, 0 },
99                                   { 0, 0, 0, 0, 0, 0 } });
100         final StateCovariance primaryCovariance = new StateCovariance(primaryCovarianceMatrixInPrimaryRTN,
101                                                                       timeOfClosestApproach, LOFType.QSW);
102         final double primaryRadius = combinedRadius / 2;
103 
104         // Define the secondary collision object
105         final Frame secondaryInertialFrame = FramesFactory.getEME2000();
106         final Orbit secondary = new CartesianOrbit(
107                 new PVCoordinates(new Vector3D(2.333465506263321e3, -1.103671212478364e6, 7.105914958099038e6),
108                                   new Vector3D(7.353740487126315e3, -1.142814049765362e3, -1.982472259113771e2)),
109                 secondaryInertialFrame, timeOfClosestApproach, mu);
110         final RealMatrix secondaryCovarianceMatrixInSecondaryRTN = new BlockRealMatrix(
111                 new double[][] { { 6.346570910720371e2, -1.962292216245289e3, 7.077413655227660e1, 0, 0, 0 },
112                                  { -1.962292216245289e3, 8.199899363150306e5, 1.139823810584350e3, 0, 0, 0 },
113                                  { 7.077413655227660e1, 1.139823810584350e3, 2.510340829074070e2, 0, 0, 0 },
114                                  { 0, 0, 0, 0, 0, 0 },
115                                  { 0, 0, 0, 0, 0, 0 },
116                                  { 0, 0, 0, 0, 0, 0 } });
117         final StateCovariance secondaryCovariance = new StateCovariance(secondaryCovarianceMatrixInSecondaryRTN,
118                                                                         timeOfClosestApproach, LOFType.QSW);
119         final double secondaryRadius = combinedRadius / 2;
120 
121         final ShortTermEncounter2DDefinition collision = new ShortTermEncounter2DDefinition(primary, primaryCovariance,
122                                                                                             primaryRadius, secondary,
123                                                                                             secondaryCovariance,
124                                                                                             secondaryRadius);
125 
126         // WHEN
127         final ProbabilityOfCollision result = method.compute(collision);
128 
129         // THEN
130         Assertions.assertEquals(0.147559, result.getValue(), 1e-6);
131     }
132 
133     @Test
134     @DisplayName("Test this method on Armellin's data and compare statistics")
135     void testReturnAcceptableStatisticsAboutMaximumProbabilityOfCollisionWithArmellinData()
136             throws IOException {
137         // GIVEN
138         final List<ArmellinDataRow> armellinDataRowList = ArmellinDataLoader.load();
139 
140         // WHEN
141         final DescriptiveStatistics statistics =
142                 ArmellinStatistics.getAlfriend1999ProbabilityOfCollisionRelativeDifferenceStatistics(
143                         armellinDataRowList);
144 
145         // THEN
146         Assertions.assertTrue(statistics.getMean() <= 8.843564833687099E-10);
147         Assertions.assertTrue(statistics.getStandardDeviation() <= 3.607777228462353E-9);
148     }
149 
150     /**
151      * This method use the data from the appendix (p.13) of "Armellin, R. (2021). Collision Avoidance Maneuver Optimization
152      * with a Multiple-Impulse Convex Formulation."
153      */
154     @Test
155     @DisplayName("Test this method on Armellin appendix test case")
156     void testComputeExpectedApproximatedProbabilityOfCollisionFromArmellinAppendixField() {
157 
158         // GIVEN
159         // Define the time of closest approach and mu
160         final Field<Binary64> field = Binary64Field.getInstance();
161 
162         final FieldAbsoluteDate<Binary64> timeOfClosestApproach = new FieldAbsoluteDate<>(field);
163 
164         final Binary64 mu = new Binary64(Constants.IERS2010_EARTH_MU);
165 
166         // Value of combined radius from Armellin's paper appendix (m)
167         final Binary64 combinedRadius = new Binary64(29.71);
168 
169         // Define the primary collision object
170         final Frame primaryInertialFrame = FramesFactory.getEME2000();
171 
172         final FieldOrbit<Binary64> primary = new FieldCartesianOrbit<>(
173                 new FieldPVCoordinates<>(
174                         new FieldVector3D<>(new Binary64(2.33052185175137e3), new Binary64(-1.10370451050201e6),
175                                             new Binary64(7.10588764299718e6)),
176                         new FieldVector3D<>(new Binary64(-7.44286282871773e3), new Binary64(-6.13734743652660e-1),
177                                             new Binary64(3.95136139293349e0))),
178                 primaryInertialFrame, timeOfClosestApproach, mu);
179 
180         final FieldMatrix<Binary64> primaryCovarianceMatrixInPrimaryRTN = new BlockFieldMatrix<>(
181                 new Binary64[][] { { new Binary64(9.31700905887535e1), new Binary64(-2.623398113500550e2),
182                                      new Binary64(2.360382173935300e1), new Binary64(0), new Binary64(0), new Binary64(0) },
183                                    { new Binary64(-2.623398113500550e2), new Binary64(1.77796454279511e4),
184                                      new Binary64(-9.331225387386501e1), new Binary64(0), new Binary64(0), new Binary64(0) },
185                                    { new Binary64(2.360382173935300e1), new Binary64(-9.331225387386501e1),
186                                      new Binary64(1.917372231880040e1), new Binary64(0), new Binary64(0), new Binary64(0) },
187                                    { new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0),
188                                      new Binary64(0) },
189                                    { new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0),
190                                      new Binary64(0) },
191                                    { new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0),
192                                      new Binary64(0) } });
193 
194         final FieldStateCovariance<Binary64> primaryCovariance =
195                 new FieldStateCovariance<>(primaryCovarianceMatrixInPrimaryRTN,
196                                            timeOfClosestApproach, LOFType.QSW_INERTIAL);
197 
198         final Binary64 primaryRadius = combinedRadius.multiply(0.5);
199 
200         // Define the secondary collision object
201         final Frame secondaryInertialFrame = FramesFactory.getEME2000();
202 
203         final FieldOrbit<Binary64> secondary = new FieldCartesianOrbit<>(
204                 new FieldPVCoordinates<>(
205                         new FieldVector3D<>(new Binary64(2.333465506263321e3), new Binary64(-1.103671212478364e6),
206                                             new Binary64(7.105914958099038e6)),
207                         new FieldVector3D<>(new Binary64(7.353740487126315e3), new Binary64(-1.142814049765362e3),
208                                             new Binary64(-1.982472259113771e2))),
209                 secondaryInertialFrame, timeOfClosestApproach, mu);
210 
211         final FieldMatrix<Binary64> secondaryCovarianceMatrixInSecondaryRTN = new BlockFieldMatrix<>(
212                 new Binary64[][] { { new Binary64(6.346570910720371e2), new Binary64(-1.962292216245289e3),
213                                      new Binary64(7.077413655227660e1), new Binary64(0), new Binary64(0), new Binary64(0) },
214                                    { new Binary64(-1.962292216245289e3), new Binary64(8.199899363150306e5),
215                                      new Binary64(1.139823810584350e3), new Binary64(0), new Binary64(0), new Binary64(0) },
216                                    { new Binary64(7.077413655227660e1), new Binary64(1.139823810584350e3),
217                                      new Binary64(2.510340829074070e2), new Binary64(0), new Binary64(0), new Binary64(0) },
218                                    { new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0),
219                                      new Binary64(0) },
220                                    { new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0),
221                                      new Binary64(0) },
222                                    { new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0),
223                                      new Binary64(0) } });
224 
225         final FieldStateCovariance<Binary64> secondaryCovariance =
226                 new FieldStateCovariance<>(secondaryCovarianceMatrixInSecondaryRTN,
227                                            timeOfClosestApproach, LOFType.QSW_INERTIAL);
228 
229         final Binary64 secondaryRadius = combinedRadius.multiply(0.5);
230 
231         final FieldShortTermEncounter2DDefinition<Binary64> collision =
232                 new FieldShortTermEncounter2DDefinition<>(primary, primaryCovariance,
233                                                           primaryRadius, secondary,
234                                                           secondaryCovariance,
235                                                           secondaryRadius);
236 
237         // WHEN
238         final FieldProbabilityOfCollision<Binary64> result = method.compute(collision);
239 
240         // THEN
241         Assertions.assertEquals(0.147559, result.getValue().getReal(), 1e-6);
242     }
243 
244     @Test
245     @DisplayName("Test this method on Armellin's data and compare statistics")
246     void testReturnAcceptableStatisticsAboutMaximumProbabilityOfCollisionWithArmellinDataField()
247             throws IOException {
248         // GIVEN
249         final List<ArmellinDataRow> armellinDataRowList = ArmellinDataLoader.load();
250 
251         // WHEN
252         final DescriptiveStatistics statistics =
253                 ArmellinStatistics.getAlfriend1999FieldProbabilityOfCollisionRelativeDifferenceStatistics(
254                         armellinDataRowList);
255 
256         // THEN
257         Assertions.assertTrue(statistics.getMean() <= 8.8446207E-10);
258         Assertions.assertTrue(statistics.getStandardDeviation() <= 3.6068271E-9);
259     }
260 
261     /**
262      * This method use the data from the appendix (p.13) of "Armellin, R. (2021). Collision Avoidance Maneuver Optimization
263      * with a Multiple-Impulse Convex Formulation."
264      */
265     @Test
266     @DisplayName("Test this method on Armellin appendix test case with taylor approximation")
267     void testReturnExpectedValueWithDerivative() {
268         // GIVEN
269         final DSFactory factory = new DSFactory(5, 5);
270 
271         // Data extracted from other test
272         final double xmNominal     = 20.711983607206943;
273         final double ymNominal     = -37.87548026356126;
274         final double sigmaXNominal = 26.841611626440486;
275         final double sigmaYNominal = 72.06451864988387;
276         final double radiusNominal = 29.71;
277 
278         final double dxm     = 10;
279         final double dym     = 18;
280         final double dSigmaX = 13;
281         final double dSigmaY = 30;
282         final double dRadius = 15;
283 
284         final DerivativeStructure xm     = factory.variable(0, xmNominal);
285         final DerivativeStructure ym     = factory.variable(1, ymNominal);
286         final DerivativeStructure sigmaX = factory.variable(2, sigmaXNominal);
287         final DerivativeStructure sigmaY = factory.variable(3, sigmaYNominal);
288         final DerivativeStructure radius = factory.variable(4, radiusNominal);
289 
290         // WHEN
291         final FieldProbabilityOfCollision<DerivativeStructure> resultNominal =
292                 method.compute(xm, ym, sigmaX, sigmaY, radius);
293         final double taylorResult =
294                 resultNominal.getValue().taylor(dxm, dym, dSigmaX, dSigmaY, dRadius);
295         final double exactResult = method.compute(xmNominal + dxm, ymNominal + dym, sigmaXNominal + dSigmaX,
296                                                   sigmaYNominal + dSigmaY, radiusNominal + dRadius).getValue();
297 
298         // THEN
299         Assertions.assertEquals(0.147559, resultNominal.getValue().getReal(), 1e-6);
300         Assertions.assertEquals(exactResult, taylorResult, 1e-3);
301     }
302 
303 }