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.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
22 import org.hipparchus.geometry.euclidean.twod.FieldVector2D;
23 import org.hipparchus.geometry.euclidean.twod.Vector2D;
24 import org.hipparchus.linear.Array2DRowFieldMatrix;
25 import org.hipparchus.linear.BlockFieldMatrix;
26 import org.hipparchus.linear.FieldLUDecomposition;
27 import org.hipparchus.linear.FieldMatrix;
28 import org.hipparchus.util.FastMath;
29 import org.hipparchus.util.MathArrays;
30 import org.hipparchus.util.MathUtils;
31 import org.orekit.errors.OrekitException;
32 import org.orekit.errors.OrekitMessages;
33 import org.orekit.frames.FieldKinematicTransform;
34 import org.orekit.frames.FieldStaticTransform;
35 import org.orekit.frames.FieldTransform;
36 import org.orekit.frames.Frame;
37 import org.orekit.frames.LOF;
38 import org.orekit.frames.LOFType;
39 import org.orekit.frames.encounter.EncounterLOF;
40 import org.orekit.frames.encounter.EncounterLOFType;
41 import org.orekit.orbits.FieldOrbit;
42 import org.orekit.orbits.OrbitType;
43 import org.orekit.orbits.PositionAngleType;
44 import org.orekit.propagation.FieldStateCovariance;
45 import org.orekit.time.FieldAbsoluteDate;
46 import org.orekit.utils.FieldPVCoordinates;
47
48 /**
49 * Defines the encounter between two collision object at time of closest approach assuming a short-term encounter model . It
50 * uses the given {@link EncounterLOFType encounter frame type} to define the encounter.
51 * <p>
52 * Both the primary and secondary collision object can be at the reference of the encounter frame, it is up to the user to
53 * choose.
54 * <p>
55 * The "reference" object is the object considered at the reference of the given encounter frame while the "other" object is
56 * the one <b>not placed</b> at the reference.
57 * <p>
58 * For example, if the user wants the primary to be at the reference of the default encounter frame, they will have to input
59 * data in the following manner:
60 * <pre>{@code
61 * final FieldShortTermEncounter2DDefinition encounter = new FieldShortTermEncounter2DDefinition<>(primaryOrbitAtTCA, primaryCovarianceAtTCA, primaryRadius, secondaryOrbitAtTCA, secondaryCovarianceAtTCA, secondaryRadius);
62 * }
63 * </pre>
64 * However, if the user wants to put the secondary at the reference and use the
65 * {@link org.orekit.frames.encounter.ValsecchiEncounterFrame Valsecchi encounter frame}, they will have to type :
66 * <pre>{@code
67 * final FieldShortTermEncounter2DDefinition encounter = new FieldShortTermEncounter2DDefinition<>(secondaryOrbitAtTCA, secondaryCovarianceAtTCA, secondaryRadius, primaryOrbitAtTCA, primaryCovarianceAtTCA, primaryRadius, EncounterLOFType.VALSECCHI_2003);
68 * }
69 * </pre>
70 * Note that in the current implementation, the shape of the collision objects is assumed to be a sphere.
71 *
72 * @author Vincent Cucchietti
73 * @since 12.0
74 * @param <T> type of the field elements
75 */
76 public class FieldShortTermEncounter2DDefinition<T extends CalculusFieldElement<T>> {
77
78 /** Default threshold below which values are considered equal to zero. */
79 private static final double DEFAULT_ZERO_THRESHOLD = 1e-15;
80
81 /** Field to which the instance elements belong. */
82 private final Field<T> instanceField;
83
84 /**
85 * Time of closest approach.
86 * <p>
87 * Commonly called TCA.
88 */
89 private final FieldAbsoluteDate<T> tca;
90
91 /** Reference collision object at time of closest approach. */
92 private final FieldOrbit<T> referenceAtTCA;
93
94 /** Reference collision object covariance matrix in its respective RTN frame. */
95 private final FieldStateCovariance<T> referenceCovariance;
96
97 /** Other collision object at time of closest approach. */
98 private final FieldOrbit<T> otherAtTCA;
99
100 /** Other collision object covariance matrix in its respective RTN frame. */
101 private final FieldStateCovariance<T> otherCovariance;
102
103 /** Combined radius (m). */
104 private final T combinedRadius;
105
106 /** Encounter local orbital frame to use. */
107 private final EncounterLOF encounterFrame;
108
109 /**
110 * Constructor.
111 *
112 * @param referenceAtTCA reference collision object orbit at time of closest approach
113 * @param referenceCovariance reference collision object covariance matrix in its respective RTN frame
114 * @param referenceRadius reference collision's equivalent sphere radius
115 * @param otherAtTCA other collision object orbit at time of closest approach
116 * @param otherCovariance other collision object covariance matrix in its respective RTN frame
117 * @param otherRadius other collision's equivalent sphere radius
118 *
119 * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
120 */
121 public FieldShortTermEncounter2DDefinition(final FieldOrbit<T> referenceAtTCA,
122 final FieldStateCovariance<T> referenceCovariance,
123 final T referenceRadius,
124 final FieldOrbit<T> otherAtTCA,
125 final FieldStateCovariance<T> otherCovariance,
126 final T otherRadius) {
127 this(referenceAtTCA, referenceCovariance, otherAtTCA, otherCovariance, referenceRadius.add(otherRadius));
128 }
129
130 /**
131 * Constructor.
132 *
133 * @param referenceAtTCA reference collision object orbit at time of closest approach
134 * @param referenceCovariance reference collision object covariance matrix in its respective RTN frame
135 * @param otherAtTCA other collision object orbit at time of closest approach
136 * @param otherCovariance other collision object covariance matrix in its respective RTN frame
137 * @param combinedRadius combined radius (m)
138 *
139 * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
140 */
141 public FieldShortTermEncounter2DDefinition(final FieldOrbit<T> referenceAtTCA,
142 final FieldStateCovariance<T> referenceCovariance,
143 final FieldOrbit<T> otherAtTCA,
144 final FieldStateCovariance<T> otherCovariance,
145 final T combinedRadius) {
146 this(referenceAtTCA, referenceCovariance, otherAtTCA, otherCovariance, combinedRadius,
147 EncounterLOFType.DEFAULT, 1e-6);
148 }
149
150 /**
151 * Constructor.
152 *
153 * @param referenceAtTCA reference collision object orbit at time of closest approach
154 * @param referenceCovariance reference collision object covariance matrix in its respective RTN frame
155 * @param referenceRadius reference collision's equivalent sphere radius
156 * @param otherAtTCA other collision object orbit at time of closest approach
157 * @param otherCovariance other collision object covariance matrix in its respective RTN frame
158 * @param otherRadius other collision's equivalent sphere radius
159 * @param encounterFrameType type of encounter frame to use
160 * @param tcaTolerance tolerance on reference and other times of closest approach difference
161 *
162 * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
163 */
164 public FieldShortTermEncounter2DDefinition(final FieldOrbit<T> referenceAtTCA,
165 final FieldStateCovariance<T> referenceCovariance,
166 final T referenceRadius,
167 final FieldOrbit<T> otherAtTCA,
168 final FieldStateCovariance<T> otherCovariance,
169 final T otherRadius,
170 final EncounterLOFType encounterFrameType,
171 final double tcaTolerance) {
172 this(referenceAtTCA, referenceCovariance, otherAtTCA, otherCovariance, referenceRadius.add(otherRadius),
173 encounterFrameType, tcaTolerance);
174 }
175
176 /**
177 * Constructor.
178 *
179 * @param referenceAtTCA reference collision object orbit at time of closest approach
180 * @param referenceCovariance reference collision object covariance matrix in its respective RTN frame
181 * @param otherAtTCA other collision object orbit at time of closest approach
182 * @param otherCovariance other collision object covariance matrix in its respective RTN frame
183 * @param combinedRadius combined radius (m)
184 * @param encounterFrameType type of encounter frame to use
185 * @param tcaTolerance tolerance on reference and other times of closest approach difference
186 *
187 * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
188 */
189 public FieldShortTermEncounter2DDefinition(final FieldOrbit<T> referenceAtTCA,
190 final FieldStateCovariance<T> referenceCovariance,
191 final FieldOrbit<T> otherAtTCA,
192 final FieldStateCovariance<T> otherCovariance,
193 final T combinedRadius,
194 final EncounterLOFType encounterFrameType,
195 final double tcaTolerance) {
196
197 if (referenceAtTCA.getDate().isCloseTo(otherAtTCA.getDate(), tcaTolerance)) {
198
199 this.tca = referenceAtTCA.getDate();
200 this.instanceField = tca.getField();
201
202 this.referenceAtTCA = referenceAtTCA;
203 this.referenceCovariance = referenceCovariance;
204
205 this.otherAtTCA = otherAtTCA;
206 this.otherCovariance = otherCovariance;
207
208 this.combinedRadius = combinedRadius;
209
210 this.encounterFrame = encounterFrameType.getFrame(otherAtTCA.getPVCoordinates());
211 } else {
212 throw new OrekitException(OrekitMessages.DIFFERENT_TIME_OF_CLOSEST_APPROACH);
213 }
214
215 }
216
217 /**
218 * Compute the squared Mahalanobis distance.
219 *
220 * @param xm other collision object projected xm position onto the collision plane in the rotated encounter frame
221 * @param ym other collision object projected ym position onto the collision plane in the rotated encounter frame
222 * @param sigmaX square root of the x-axis eigen value of the diagonalized combined covariance matrix projected onto the
223 * collision plane
224 * @param sigmaY square root of the y-axis eigen value of the diagonalized combined covariance matrix projected onto the
225 * collision plane
226 * @param <T> type of the field elements
227 *
228 * @return squared Mahalanobis distance
229 */
230 public static <T extends CalculusFieldElement<T>> T computeSquaredMahalanobisDistance(final T xm, final T ym,
231 final T sigmaX, final T sigmaY) {
232
233 final T[][] positionData = MathArrays.buildArray(xm.getField(), 2, 1);
234 positionData[0][0] = xm;
235 positionData[1][0] = ym;
236 final FieldVector2D<T> position = new FieldVector2D<>(xm, ym);
237
238 final T[][] covarianceMatrixData = MathArrays.buildArray(sigmaX.getField(), 2, 2);
239 covarianceMatrixData[0][0] = sigmaX.multiply(sigmaX);
240 covarianceMatrixData[1][1] = sigmaY.multiply(sigmaY);
241 final FieldMatrix<T> covariance = new BlockFieldMatrix<>(covarianceMatrixData);
242
243 return computeSquaredMahalanobisDistance(position, covariance);
244 }
245
246 /**
247 * Compute the squared Mahalanobis distance.
248 *
249 * @param otherPosition other collision object projected position onto the collision plane in the rotated encounter
250 * frame
251 * @param covarianceMatrix combined covariance matrix projected onto the collision plane and diagonalized
252 * @param <T> type of the field elements
253 *
254 * @return squared Mahalanobis distance
255 */
256 public static <T extends CalculusFieldElement<T>> T computeSquaredMahalanobisDistance(
257 final FieldVector2D<T> otherPosition,
258 final FieldMatrix<T> covarianceMatrix) {
259
260 final FieldMatrix<T> covarianceMatrixInverse = new FieldLUDecomposition<>(covarianceMatrix).getSolver().getInverse();
261
262 final FieldMatrix<T> otherPositionOnCollisionPlaneMatrix = new Array2DRowFieldMatrix<>(otherPosition.toArray());
263
264 return otherPositionOnCollisionPlaneMatrix.transposeMultiply(
265 covarianceMatrixInverse.multiply(otherPositionOnCollisionPlaneMatrix)).getEntry(0, 0);
266 }
267
268 /**
269 * Compute the other collision position and velocity relative to the reference collision object. Expressed in the
270 * reference collision object inertial frame.
271 *
272 * @return other collision position and velocity relative to the reference collision object, expressed in the reference
273 * collision object inertial frame.
274 */
275 public FieldPVCoordinates<T> computeOtherRelativeToReferencePVInReferenceInertial() {
276
277 // Extract reference inertial frame
278 final Frame referenceInertial = referenceAtTCA.getFrame();
279
280 // Get PVCoordinates in the same frame
281 final FieldPVCoordinates<T> referencePV = referenceAtTCA.getPVCoordinates();
282 final FieldKinematicTransform<T> kinematicTransform = otherAtTCA.getFrame()
283 .getKinematicTransformTo(referenceInertial, referenceAtTCA.getDate());
284 final FieldPVCoordinates<T> otherPVInReferenceInertial = kinematicTransform
285 .transformOnlyPV(otherAtTCA.getPVCoordinates());
286
287 // Create relative pv expressed in the reference inertial frame
288 final FieldVector3D<T> relativePosition = otherPVInReferenceInertial.getPosition()
289 .subtract(referencePV.getPosition());
290 final FieldVector3D<T> relativeVelocity = otherPVInReferenceInertial.getVelocity()
291 .subtract(referencePV.getVelocity());
292
293 return new FieldPVCoordinates<>(relativePosition, relativeVelocity);
294 }
295
296 /**
297 * Compute the projection matrix from the reference collision object inertial frame to the collision plane.
298 * <p>
299 * Note that this matrix will only rotate from the reference collision object inertial frame to the encounter frame and
300 * project onto the collision plane, this is only a rotation.
301 * </p>
302 *
303 * @return projection matrix from the reference collision object inertial frame to the collision plane
304 */
305 public FieldMatrix<T> computeReferenceInertialToCollisionPlaneProjectionMatrix() {
306
307 // Create transform from reference inertial frame to encounter local orbital frame
308 final FieldStaticTransform<T> referenceInertialToEncounterFrameTransform =
309 FieldStaticTransform.compose(tca,
310 computeReferenceInertialToReferenceTNWTransform(),
311 computeReferenceTNWToEncounterFrameTransform());
312
313 // Create rotation matrix from reference inertial frame to encounter local orbital frame
314 final FieldMatrix<T> referenceInertialToEncounterFrameRotationMatrix =
315 new Array2DRowFieldMatrix<>(referenceInertialToEncounterFrameTransform.getRotation().getMatrix());
316
317 // Create projection matrix from encounter frame to collision plane
318 final FieldMatrix<T> encounterFrameToCollisionPlaneProjectionMatrix =
319 encounterFrame.computeProjectionMatrix(tca.getField());
320
321 // Create projection matrix from reference inertial frame to collision plane
322 return encounterFrameToCollisionPlaneProjectionMatrix.multiply(referenceInertialToEncounterFrameRotationMatrix);
323 }
324
325 /**
326 * Compute the combined covariance matrix diagonalized and projected onto the collision plane.
327 * <p>
328 * Diagonalize projected positional covariance matrix in a specific manner to have
329 * <var>σ<sub>xx</sub><sup>2</sup> ≤ σ<sub>yy</sub><sup>2</sup></var>.
330 *
331 * @return combined covariance matrix diagonalized and projected onto the collision plane
332 */
333 public FieldMatrix<T> computeProjectedAndDiagonalizedCombinedPositionalCovarianceMatrix() {
334
335 final FieldMatrix<T> covarianceMatrixToDiagonalize = computeProjectedCombinedPositionalCovarianceMatrix();
336
337 final T sigmaXSquared = covarianceMatrixToDiagonalize.getEntry(0, 0);
338 final T sigmaYSquared = covarianceMatrixToDiagonalize.getEntry(1, 1);
339
340 final T crossTerm = covarianceMatrixToDiagonalize.getEntry(0, 1);
341 final T recurrentTerm = sigmaXSquared.subtract(sigmaYSquared).multiply(0.5).pow(2)
342 .add(crossTerm.square()).sqrt();
343
344 final T eigenValueX = sigmaXSquared.add(sigmaYSquared).multiply(0.5).subtract(recurrentTerm);
345 final T eigenValueY = sigmaXSquared.add(sigmaYSquared).multiply(0.5).add(recurrentTerm);
346
347 final FieldMatrix<T> projectedAndDiagonalizedCombinedPositionalCovarianceMatrix =
348 new BlockFieldMatrix<>(instanceField, 2, 2);
349 projectedAndDiagonalizedCombinedPositionalCovarianceMatrix.setEntry(0, 0, eigenValueX);
350 projectedAndDiagonalizedCombinedPositionalCovarianceMatrix.setEntry(0, 1, instanceField.getZero());
351 projectedAndDiagonalizedCombinedPositionalCovarianceMatrix.setEntry(1, 0, instanceField.getZero());
352 projectedAndDiagonalizedCombinedPositionalCovarianceMatrix.setEntry(1, 1, eigenValueY);
353
354 return projectedAndDiagonalizedCombinedPositionalCovarianceMatrix;
355 }
356
357 /**
358 * Compute the projected combined covariance matrix onto the collision plane.
359 *
360 * @return projected combined covariance matrix onto the collision plane
361 */
362 public FieldMatrix<T> computeProjectedCombinedPositionalCovarianceMatrix() {
363
364 // Compute the positional covariance in the encounter local orbital frame
365 final FieldMatrix<T> combinedPositionalCovarianceMatrixInEncounterFrame =
366 computeCombinedCovarianceInEncounterFrame().getMatrix().getSubMatrix(0, 2, 0, 2);
367
368 // Project it onto the collision plane
369 return encounterFrame.projectOntoCollisionPlane(combinedPositionalCovarianceMatrixInEncounterFrame);
370 }
371
372 /**
373 * Compute the combined covariance expressed in the encounter frame.
374 *
375 * @return combined covariance expressed in the encounter frame
376 */
377 public FieldStateCovariance<T> computeCombinedCovarianceInEncounterFrame() {
378 return computeCombinedCovarianceInReferenceTNW().changeCovarianceFrame(referenceAtTCA, encounterFrame);
379 }
380
381 /**
382 * Compute the other collision object {@link FieldVector2D position} projected onto the collision plane.
383 *
384 * @return other collision object position projected onto the collision plane
385 */
386 public FieldVector2D<T> computeOtherPositionInCollisionPlane() {
387
388 // Express other in reference inertial
389 final FieldVector3D<T> otherInReferenceInertial = otherAtTCA.getPosition(referenceAtTCA.getFrame());
390
391 // Express other in reference TNW local orbital frame
392 final FieldVector3D<T> otherPositionInReferenceTNW =
393 computeReferenceInertialToReferenceTNWTransform().transformPosition(otherInReferenceInertial);
394
395 // Express other in encounter local orbital frame
396 final FieldVector3D<T> otherPositionInEncounterFrame =
397 computeReferenceTNWToEncounterFrameTransform().transformPosition(otherPositionInReferenceTNW);
398
399 return encounterFrame.projectOntoCollisionPlane(otherPositionInEncounterFrame);
400
401 }
402
403 /**
404 * Compute the other collision object {@link FieldVector2D position} in the rotated collision plane.
405 * <p>
406 * Uses a default zero threshold of 1e-15.
407 * <p>
408 * The coordinates are often noted xm and ym in probability of collision related papers.
409 * </p>
410 * <p>
411 * The mentioned rotation concerns the rotation that diagonalize the combined covariance matrix inside the collision
412 * plane.
413 * </p>
414 *
415 * @return other collision object position in the rotated collision plane
416 */
417 public FieldVector2D<T> computeOtherPositionInRotatedCollisionPlane() {
418 return computeOtherPositionInRotatedCollisionPlane(DEFAULT_ZERO_THRESHOLD);
419
420 }
421
422 /**
423 * Compute the other collision object {@link Vector2D position} in the rotated collision plane.
424 * <p>
425 * The coordinates are often noted xm and ym in probability of collision related papers.
426 * <p>
427 * The mentioned rotation concerns the rotation that diagonalize the combined covariance matrix inside the collision
428 * plane.
429 *
430 * @param zeroThreshold threshold below which values are considered equal to zero
431 *
432 * @return other collision object position in the rotated collision plane
433 */
434 public FieldVector2D<T> computeOtherPositionInRotatedCollisionPlane(final double zeroThreshold) {
435
436 // Project the other position onto the collision plane
437 final FieldMatrix<T> otherPositionInCollisionPlaneMatrix =
438 new Array2DRowFieldMatrix<>(computeOtherPositionInCollisionPlane().toArray());
439
440 // Express other in the rotated collision plane
441 final FieldMatrix<T> otherPositionRotatedInCollisionPlane =
442 computeEncounterPlaneRotationMatrix(zeroThreshold).multiply(otherPositionInCollisionPlaneMatrix);
443
444 return new FieldVector2D<>(otherPositionRotatedInCollisionPlane.getColumn(0));
445
446 }
447
448 /**
449 * Compute the Encounter duration (s) evaluated using Coppola's formula described in : "COPPOLA, Vincent, et al.
450 * Evaluating the short encounter assumption of the probability of collision formula. 2012."
451 * <p>
452 * This method is to be used to check the validity of the short-term encounter model. The user is expected to compare the
453 * computed duration with the orbital period from both objects and draw its own conclusions.
454 * <p>
455 * It uses γ = 1e-16 as the resolution of a double is nearly 1e-16 so γ smaller than that are not meaningful to compute.
456 *
457 * @return encounter duration (s) evaluated using Coppola's formula
458 */
459 public T computeCoppolaEncounterDuration() {
460
461 // Default value for γ = 1e-16
462 final T DEFAULT_ALPHA_C = instanceField.getZero().newInstance(5.864);
463
464 final FieldMatrix<T> combinedPositionalCovarianceMatrix = computeCombinedCovarianceInEncounterFrame()
465 .getMatrix().getSubMatrix(0, 2, 0, 2);
466
467 // Extract off-plane cross-term matrix
468 final FieldMatrix<T> projectionMatrix = encounterFrame.computeProjectionMatrix(instanceField);
469 final FieldMatrix<T> axisNormalToCollisionPlane =
470 new Array2DRowFieldMatrix<>(encounterFrame.getAxisNormalToCollisionPlane(instanceField).toArray());
471 final FieldMatrix<T> offPlaneCrossTermMatrix =
472 projectionMatrix.multiply(combinedPositionalCovarianceMatrix.multiply(axisNormalToCollisionPlane));
473
474 // Covariance sub-matrix of the in-plane terms
475 final FieldMatrix<T> probabilityDensity =
476 encounterFrame.projectOntoCollisionPlane(combinedPositionalCovarianceMatrix);
477 final FieldMatrix<T> probabilityDensityInverse =
478 new FieldLUDecomposition<>(probabilityDensity).getSolver().getInverse();
479
480 // Recurrent term in Coppola's paper : bᵀb
481 final FieldMatrix<T> b = offPlaneCrossTermMatrix.transposeMultiply(probabilityDensityInverse).transpose();
482 final T recurrentTerm = b.multiplyTransposed(b).getEntry(0, 0);
483
484 // Position uncertainty normal to collision plane
485 final T sigmaSqNormalToPlan = axisNormalToCollisionPlane.transposeMultiply(
486 combinedPositionalCovarianceMatrix.multiply(axisNormalToCollisionPlane)).getEntry(0, 0);
487 final T sigmaV = sigmaSqNormalToPlan.subtract(b.multiplyTransposed(offPlaneCrossTermMatrix).getEntry(0, 0))
488 .sqrt();
489
490 final T relativeVelocity = computeOtherRelativeToReferencePVInReferenceInertial().getVelocity().getNorm();
491
492 return DEFAULT_ALPHA_C.multiply(sigmaV).multiply(2 * FastMath.sqrt(2)).add(
493 combinedRadius.multiply(recurrentTerm.add(1).sqrt().add(recurrentTerm.sqrt()))).divide(relativeVelocity);
494 }
495
496 /**
497 * Compute the miss distance at time of closest approach.
498 *
499 * @return miss distance
500 */
501 public T computeMissDistance() {
502
503 // Get positions expressed in the same frame at time of closest approach
504 final FieldVector3D<T> referencePositionAtTCA = referenceAtTCA.getPosition();
505 final FieldVector3D<T> otherPositionAtTCA = otherAtTCA.getPosition(referenceAtTCA.getFrame());
506
507 // Compute relative position
508 final FieldVector3D<T> relativePosition = otherPositionAtTCA.subtract(referencePositionAtTCA);
509
510 return relativePosition.getNorm();
511 }
512
513 /**
514 * Compute the Mahalanobis distance computed with the other collision object projected onto the collision plane (commonly
515 * called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix is
516 * diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
517 * <p>
518 * Uses a default zero threshold of 1e-15 for the computation of the diagonalizing of the projected covariance matrix.
519 *
520 * @return Mahalanobis distance between the reference and other collision object
521 *
522 * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
523 */
524 public T computeMahalanobisDistance() {
525 return computeMahalanobisDistance(DEFAULT_ZERO_THRESHOLD);
526 }
527
528 /**
529 * Compute the Mahalanobis distance computed with the other collision object projected onto the collision plane (commonly
530 * called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix is
531 * diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
532 *
533 * @param zeroThreshold threshold below which values are considered equal to zero
534 *
535 * @return Mahalanobis distance between the reference and other collision object
536 *
537 * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
538 */
539 public T computeMahalanobisDistance(final double zeroThreshold) {
540 return computeSquaredMahalanobisDistance(zeroThreshold).sqrt();
541 }
542
543 /**
544 * Compute the squared Mahalanobis distance computed with the other collision object projected onto the collision plane
545 * (commonly called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix
546 * is diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
547 * <p>
548 * Uses a default zero threshold of 1e-15 for the computation of the diagonalizing of the projected covariance matrix.
549 *
550 * @return squared Mahalanobis distance between the reference and other collision object
551 *
552 * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
553 */
554 public T computeSquaredMahalanobisDistance() {
555 return computeSquaredMahalanobisDistance(DEFAULT_ZERO_THRESHOLD);
556 }
557
558 /**
559 * Compute the squared Mahalanobis distance computed with the other collision object projected onto the collision plane
560 * (commonly called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix
561 * is diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
562 *
563 * @param zeroThreshold threshold below which values are considered equal to zero
564 *
565 * @return squared Mahalanobis distance between the reference and other collision object
566 *
567 * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
568 */
569 public T computeSquaredMahalanobisDistance(final double zeroThreshold) {
570
571 final FieldMatrix<T> otherPositionAfterRotationInCollisionPlane =
572 new Array2DRowFieldMatrix<>(computeOtherPositionInRotatedCollisionPlane(zeroThreshold).toArray());
573
574 final FieldMatrix<T> inverseCovarianceMatrix =
575 new FieldLUDecomposition<>(
576 computeProjectedAndDiagonalizedCombinedPositionalCovarianceMatrix()).getSolver()
577 .getInverse();
578
579 return otherPositionAfterRotationInCollisionPlane.transpose().multiply(
580 inverseCovarianceMatrix.multiply(
581 otherPositionAfterRotationInCollisionPlane))
582 .getEntry(0, 0);
583 }
584
585 /** Get new encounter instance.
586 * @return new encounter instance
587 */
588 public ShortTermEncounter2DDefinition toEncounter() {
589 return new ShortTermEncounter2DDefinition(referenceAtTCA.toOrbit(), referenceCovariance.toStateCovariance(),
590 otherAtTCA.toOrbit(), otherCovariance.toStateCovariance(),
591 combinedRadius.getReal());
592 }
593
594 /**
595 * Takes both covariance matrices (expressed in their respective RTN local orbital frame) from reference and other
596 * collision object with which this instance was created and sum them in the reference collision object TNW local orbital
597 * frame.
598 *
599 * @return combined covariance matrix expressed in the reference collision object TNW local orbital frame
600 */
601 public FieldStateCovariance<T> computeCombinedCovarianceInReferenceTNW() {
602
603 // Express reference covariance in reference TNW local orbital frame
604 final FieldMatrix<T> referenceCovarianceMatrixInTNW =
605 referenceCovariance.changeCovarianceFrame(referenceAtTCA, LOFType.TNW_INERTIAL).getMatrix();
606
607 // Express other covariance in reference inertial frame
608 final FieldMatrix<T> otherCovarianceMatrixInReferenceInertial =
609 otherCovariance.changeCovarianceFrame(otherAtTCA, referenceAtTCA.getFrame()).getMatrix();
610
611 final FieldStateCovariance<T> otherCovarianceInReferenceInertial = new FieldStateCovariance<>(
612 otherCovarianceMatrixInReferenceInertial, tca, referenceAtTCA.getFrame(),
613 OrbitType.CARTESIAN, PositionAngleType.MEAN);
614
615 // Express other covariance in reference TNW local orbital frame
616 final FieldMatrix<T> otherCovarianceMatrixInReferenceTNW = otherCovarianceInReferenceInertial.changeCovarianceFrame(
617 referenceAtTCA, LOFType.TNW_INERTIAL).getMatrix();
618
619 // Return the combined covariance expressed in the reference TNW local orbital frame
620 return new FieldStateCovariance<>(referenceCovarianceMatrixInTNW.add(otherCovarianceMatrixInReferenceTNW), tca,
621 LOFType.TNW_INERTIAL);
622 }
623
624 /**
625 * Compute the {@link FieldTransform transform} from the reference collision object inertial frame of reference to its
626 * TNW local orbital frame.
627 *
628 * @return transform from the reference collision object inertial frame of reference to its TNW local orbital frame
629 */
630 private FieldTransform<T> computeReferenceInertialToReferenceTNWTransform() {
631 return LOFType.TNW.transformFromInertial(tca, referenceAtTCA.getPVCoordinates());
632 }
633
634 /**
635 * Compute the {@link FieldTransform transform} from the reference collision object TNW local orbital frame to the
636 * encounter frame.
637 *
638 * @return transform from the reference collision object TNW local orbital frame to the encounter frame
639 */
640 private FieldTransform<T> computeReferenceTNWToEncounterFrameTransform() {
641 return LOF.transformFromLOFInToLOFOut(LOFType.TNW_INERTIAL, encounterFrame, tca,
642 referenceAtTCA.getPVCoordinates());
643 }
644
645 /**
646 * Compute the rotation matrix that diagonalize the combined positional covariance matrix projected onto the collision
647 * plane.
648 *
649 * @param zeroThreshold threshold below which values are considered equal to zero
650 *
651 * @return rotation matrix that diagonalize the combined covariance matrix projected onto the collision plane
652 */
653 private FieldMatrix<T> computeEncounterPlaneRotationMatrix(final double zeroThreshold) {
654
655 final FieldMatrix<T> combinedCovarianceMatrixInEncounterFrame =
656 computeCombinedCovarianceInEncounterFrame().getMatrix();
657
658 final FieldMatrix<T> combinedPositionalCovarianceMatrixProjectedOntoBPlane =
659 encounterFrame.projectOntoCollisionPlane(
660 combinedCovarianceMatrixInEncounterFrame.getSubMatrix(0, 2, 0, 2));
661
662 final T sigmaXSquared = combinedPositionalCovarianceMatrixProjectedOntoBPlane.getEntry(0, 0);
663 final T sigmaYSquared = combinedPositionalCovarianceMatrixProjectedOntoBPlane.getEntry(1, 1);
664 final T crossTerm = combinedPositionalCovarianceMatrixProjectedOntoBPlane.getEntry(0, 1);
665 final T correlation = crossTerm.divide(sigmaXSquared.multiply(sigmaYSquared).sqrt());
666
667 // If the matrix is not initially diagonalized
668 final T theta;
669 if (FastMath.abs(crossTerm).getReal() > zeroThreshold) {
670 final T recurrentTerm = sigmaYSquared.subtract(sigmaXSquared).divide(crossTerm.multiply(2));
671 theta = recurrentTerm.subtract(correlation.sign().multiply(recurrentTerm.pow(2).add(1).sqrt())).atan();
672 }
673 // Else, the matrix is already diagonalized
674 else {
675 // Rotation in order to have sigmaXSquared < sigmaYSquared
676 if (sigmaXSquared.subtract(sigmaYSquared).getReal() > 0) {
677 theta = tca.getField().getZero().newInstance(MathUtils.SEMI_PI);
678 }
679 // Else, there is no need for a rotation
680 else {
681 theta = tca.getField().getZero();
682 }
683 }
684
685 final T cosTheta = theta.cos();
686 final T sinTheta = theta.sin();
687 final Array2DRowFieldMatrix<T> rotationMatrix = new Array2DRowFieldMatrix<>(tca.getField(), 2, 2);
688 rotationMatrix.setEntry(0, 0, cosTheta);
689 rotationMatrix.setEntry(0, 1, sinTheta);
690 rotationMatrix.setEntry(1, 0, sinTheta.negate());
691 rotationMatrix.setEntry(1, 1, cosTheta);
692
693 return rotationMatrix;
694 }
695
696 /**
697 * Get the Time of Closest Approach.
698 * <p>
699 * Commonly called TCA.
700 *
701 * @return time of closest approach
702 */
703 public FieldAbsoluteDate<T> getTca() {
704 return tca;
705 }
706
707 /** Get reference's orbit at time of closest approach.
708 * @return reference's orbit at time of closest approach
709 */
710 public FieldOrbit<T> getReferenceAtTCA() {
711 return referenceAtTCA;
712 }
713
714 /** Get other's orbit at time of closest approach.
715 * @return other's orbit at time of closest approach
716 */
717 public FieldOrbit<T> getOtherAtTCA() {
718 return otherAtTCA;
719 }
720
721 /** Get reference's covariance.
722 * @return reference's covariance
723 */
724 public FieldStateCovariance<T> getReferenceCovariance() {
725 return referenceCovariance;
726 }
727
728 /** Get other's covariance.
729 * @return other's covariance
730 */
731 public FieldStateCovariance<T> getOtherCovariance() {
732 return otherCovariance;
733 }
734
735 /** Get combined radius.
736 * @return combined radius (m)
737 */
738 public T getCombinedRadius() {
739 return combinedRadius;
740 }
741
742 /** Get encounter local orbital frame.
743 * @return encounter local orbital frame
744 */
745 public EncounterLOF getEncounterFrame() {
746 return encounterFrame;
747 }
748
749 }