FieldShortTermEncounter2DDefinition.java

  1. /* Copyright 2002-2024 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. import org.hipparchus.CalculusFieldElement;
  19. import org.hipparchus.Field;
  20. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  21. import org.hipparchus.geometry.euclidean.twod.FieldVector2D;
  22. import org.hipparchus.geometry.euclidean.twod.Vector2D;
  23. import org.hipparchus.linear.Array2DRowFieldMatrix;
  24. import org.hipparchus.linear.BlockFieldMatrix;
  25. import org.hipparchus.linear.FieldLUDecomposition;
  26. import org.hipparchus.linear.FieldMatrix;
  27. import org.hipparchus.util.FastMath;
  28. import org.hipparchus.util.MathArrays;
  29. import org.hipparchus.util.MathUtils;
  30. import org.orekit.errors.OrekitException;
  31. import org.orekit.errors.OrekitMessages;
  32. import org.orekit.frames.FieldKinematicTransform;
  33. import org.orekit.frames.FieldStaticTransform;
  34. import org.orekit.frames.FieldTransform;
  35. import org.orekit.frames.Frame;
  36. import org.orekit.frames.LOF;
  37. import org.orekit.frames.LOFType;
  38. import org.orekit.frames.encounter.EncounterLOF;
  39. import org.orekit.frames.encounter.EncounterLOFType;
  40. import org.orekit.orbits.FieldOrbit;
  41. import org.orekit.orbits.OrbitType;
  42. import org.orekit.orbits.PositionAngleType;
  43. import org.orekit.propagation.FieldStateCovariance;
  44. import org.orekit.time.FieldAbsoluteDate;
  45. import org.orekit.utils.FieldPVCoordinates;

  46. /**
  47.  * Defines the encounter between two collision object at time of closest approach assuming a short-term encounter model . It
  48.  * uses the given {@link EncounterLOFType encounter frame type} to define the encounter.
  49.  * <p>
  50.  * Both the primary and secondary collision object can be at the reference of the encounter frame, it is up to the user to
  51.  * choose.
  52.  * <p>
  53.  * The "reference" object is the object considered at the reference of the given encounter frame while the "other" object is
  54.  * the one <b>not placed</b> at the reference.
  55.  * <p>
  56.  * For example, if the user wants the primary to be at the reference of the default encounter frame, they will have to input
  57.  * data in the following manner:
  58.  * <pre>{@code
  59.  * final FieldShortTermEncounter2DDefinition encounter = new FieldShortTermEncounter2DDefinition<>(primaryOrbitAtTCA, primaryCovarianceAtTCA, primaryRadius, secondaryOrbitAtTCA, secondaryCovarianceAtTCA, secondaryRadius);
  60.  *  }
  61.  * </pre>
  62.  * However, if the user wants to put the secondary at the reference and use the
  63.  * {@link org.orekit.frames.encounter.ValsecchiEncounterFrame Valsecchi encounter frame}, they will have to type :
  64.  * <pre>{@code
  65.  * final FieldShortTermEncounter2DDefinition encounter = new FieldShortTermEncounter2DDefinition<>(secondaryOrbitAtTCA, secondaryCovarianceAtTCA, secondaryRadius, primaryOrbitAtTCA, primaryCovarianceAtTCA, primaryRadius, EncounterLOFType.VALSECCHI_2003);
  66.  *  }
  67.  * </pre>
  68.  * Note that in the current implementation, the shape of the collision objects is assumed to be a sphere.
  69.  *
  70.  * @author Vincent Cucchietti
  71.  * @since 12.0
  72.  * @param <T> type of the field elements
  73.  */
  74. public class FieldShortTermEncounter2DDefinition<T extends CalculusFieldElement<T>> {

  75.     /** Default threshold below which values are considered equal to zero. */
  76.     private static final double DEFAULT_ZERO_THRESHOLD = 1e-15;

  77.     /** Field to which the instance elements belong. */
  78.     private final Field<T> instanceField;

  79.     /**
  80.      * Time of closest approach.
  81.      * <p>
  82.      * Commonly called TCA.
  83.      */
  84.     private final FieldAbsoluteDate<T> tca;

  85.     /** Reference collision object at time of closest approach. */
  86.     private final FieldOrbit<T> referenceAtTCA;

  87.     /** Reference collision object covariance matrix in its respective RTN frame. */
  88.     private final FieldStateCovariance<T> referenceCovariance;

  89.     /** Other collision object at time of closest approach. */
  90.     private final FieldOrbit<T> otherAtTCA;

  91.     /** Other collision object covariance matrix in its respective RTN frame. */
  92.     private final FieldStateCovariance<T> otherCovariance;

  93.     /** Combined radius (m). */
  94.     private final T combinedRadius;

  95.     /** Encounter local orbital frame to use. */
  96.     private final EncounterLOF encounterFrame;

  97.     /**
  98.      * Constructor.
  99.      *
  100.      * @param referenceAtTCA reference collision object orbit at time of closest approach
  101.      * @param referenceCovariance reference collision object covariance matrix in its respective RTN frame
  102.      * @param referenceRadius reference collision's equivalent sphere radius
  103.      * @param otherAtTCA other collision object  orbit at time of closest approach
  104.      * @param otherCovariance other collision object covariance matrix in its respective RTN frame
  105.      * @param otherRadius other collision's equivalent sphere radius
  106.      *
  107.      * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
  108.      */
  109.     public FieldShortTermEncounter2DDefinition(final FieldOrbit<T> referenceAtTCA,
  110.                                                final FieldStateCovariance<T> referenceCovariance,
  111.                                                final T referenceRadius,
  112.                                                final FieldOrbit<T> otherAtTCA,
  113.                                                final FieldStateCovariance<T> otherCovariance,
  114.                                                final T otherRadius) {
  115.         this(referenceAtTCA, referenceCovariance, otherAtTCA, otherCovariance, referenceRadius.add(otherRadius));
  116.     }

  117.     /**
  118.      * Constructor.
  119.      *
  120.      * @param referenceAtTCA reference collision object orbit at time of closest approach
  121.      * @param referenceCovariance reference collision object covariance matrix in its respective RTN frame
  122.      * @param otherAtTCA other collision object  orbit at time of closest approach
  123.      * @param otherCovariance other collision object covariance matrix in its respective RTN frame
  124.      * @param combinedRadius combined radius (m)
  125.      *
  126.      * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
  127.      */
  128.     public FieldShortTermEncounter2DDefinition(final FieldOrbit<T> referenceAtTCA,
  129.                                                final FieldStateCovariance<T> referenceCovariance,
  130.                                                final FieldOrbit<T> otherAtTCA,
  131.                                                final FieldStateCovariance<T> otherCovariance,
  132.                                                final T combinedRadius) {
  133.         this(referenceAtTCA, referenceCovariance, otherAtTCA, otherCovariance, combinedRadius,
  134.              EncounterLOFType.DEFAULT, 1e-6);
  135.     }

  136.     /**
  137.      * Constructor.
  138.      *
  139.      * @param referenceAtTCA reference collision object orbit at time of closest approach
  140.      * @param referenceCovariance reference collision object covariance matrix in its respective RTN frame
  141.      * @param referenceRadius reference collision's equivalent sphere radius
  142.      * @param otherAtTCA other collision object  orbit at time of closest approach
  143.      * @param otherCovariance other collision object covariance matrix in its respective RTN frame
  144.      * @param otherRadius other collision's equivalent sphere radius
  145.      * @param encounterFrameType type of encounter frame to use
  146.      * @param tcaTolerance tolerance on reference and other times of closest approach difference
  147.      *
  148.      * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
  149.      */
  150.     public FieldShortTermEncounter2DDefinition(final FieldOrbit<T> referenceAtTCA,
  151.                                                final FieldStateCovariance<T> referenceCovariance,
  152.                                                final T referenceRadius,
  153.                                                final FieldOrbit<T> otherAtTCA,
  154.                                                final FieldStateCovariance<T> otherCovariance,
  155.                                                final T otherRadius,
  156.                                                final EncounterLOFType encounterFrameType,
  157.                                                final double tcaTolerance) {
  158.         this(referenceAtTCA, referenceCovariance, otherAtTCA, otherCovariance, referenceRadius.add(otherRadius),
  159.              encounterFrameType, tcaTolerance);
  160.     }

  161.     /**
  162.      * Constructor.
  163.      *
  164.      * @param referenceAtTCA reference collision object orbit at time of closest approach
  165.      * @param referenceCovariance reference collision object covariance matrix in its respective RTN frame
  166.      * @param otherAtTCA other collision object  orbit at time of closest approach
  167.      * @param otherCovariance other collision object covariance matrix in its respective RTN frame
  168.      * @param combinedRadius combined radius (m)
  169.      * @param encounterFrameType type of encounter frame to use
  170.      * @param tcaTolerance tolerance on reference and other times of closest approach difference
  171.      *
  172.      * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
  173.      */
  174.     public FieldShortTermEncounter2DDefinition(final FieldOrbit<T> referenceAtTCA,
  175.                                                final FieldStateCovariance<T> referenceCovariance,
  176.                                                final FieldOrbit<T> otherAtTCA,
  177.                                                final FieldStateCovariance<T> otherCovariance,
  178.                                                final T combinedRadius,
  179.                                                final EncounterLOFType encounterFrameType,
  180.                                                final double tcaTolerance) {

  181.         if (referenceAtTCA.getDate().isCloseTo(otherAtTCA.getDate(), tcaTolerance)) {

  182.             this.tca           = referenceAtTCA.getDate();
  183.             this.instanceField = tca.getField();

  184.             this.referenceAtTCA      = referenceAtTCA;
  185.             this.referenceCovariance = referenceCovariance;

  186.             this.otherAtTCA      = otherAtTCA;
  187.             this.otherCovariance = otherCovariance;

  188.             this.combinedRadius = combinedRadius;

  189.             this.encounterFrame = encounterFrameType.getFrame(otherAtTCA.getPVCoordinates());
  190.         } else {
  191.             throw new OrekitException(OrekitMessages.DIFFERENT_TIME_OF_CLOSEST_APPROACH);
  192.         }

  193.     }

  194.     /**
  195.      * Compute the squared Mahalanobis distance.
  196.      *
  197.      * @param xm other collision object projected xm position onto the collision plane in the rotated encounter frame
  198.      * @param ym other collision object projected ym position onto the collision plane in the rotated encounter frame
  199.      * @param sigmaX square root of the x-axis eigen value of the diagonalized combined covariance matrix projected onto the
  200.      * collision plane
  201.      * @param sigmaY square root of the y-axis eigen value of the diagonalized combined covariance matrix projected onto the
  202.      * collision plane
  203.      * @param <T> type of the field elements
  204.      *
  205.      * @return squared Mahalanobis distance
  206.      */
  207.     public static <T extends CalculusFieldElement<T>> T computeSquaredMahalanobisDistance(final T xm, final T ym,
  208.                                                                                           final T sigmaX, final T sigmaY) {

  209.         final T[][] positionData = MathArrays.buildArray(xm.getField(), 2, 1);
  210.         positionData[0][0] = xm;
  211.         positionData[1][0] = ym;
  212.         final FieldVector2D<T> position = new FieldVector2D<>(xm, ym);

  213.         final T[][] covarianceMatrixData = MathArrays.buildArray(sigmaX.getField(), 2, 2);
  214.         covarianceMatrixData[0][0] = sigmaX.multiply(sigmaX);
  215.         covarianceMatrixData[1][1] = sigmaY.multiply(sigmaY);
  216.         final FieldMatrix<T> covariance = new BlockFieldMatrix<>(covarianceMatrixData);

  217.         return computeSquaredMahalanobisDistance(position, covariance);
  218.     }

  219.     /**
  220.      * Compute the squared Mahalanobis distance.
  221.      *
  222.      * @param otherPosition other collision object projected position onto the collision plane in the rotated encounter
  223.      * frame
  224.      * @param covarianceMatrix combined covariance matrix projected onto the collision plane and diagonalized
  225.      * @param <T> type of the field elements
  226.      *
  227.      * @return squared Mahalanobis distance
  228.      */
  229.     public static <T extends CalculusFieldElement<T>> T computeSquaredMahalanobisDistance(
  230.             final FieldVector2D<T> otherPosition,
  231.             final FieldMatrix<T> covarianceMatrix) {

  232.         final FieldMatrix<T> covarianceMatrixInverse = new FieldLUDecomposition<>(covarianceMatrix).getSolver().getInverse();

  233.         final FieldMatrix<T> otherPositionOnCollisionPlaneMatrix = new Array2DRowFieldMatrix<>(otherPosition.toArray());

  234.         return otherPositionOnCollisionPlaneMatrix.transposeMultiply(
  235.                 covarianceMatrixInverse.multiply(otherPositionOnCollisionPlaneMatrix)).getEntry(0, 0);
  236.     }

  237.     /**
  238.      * Compute the other collision position and velocity relative to the reference collision object. Expressed in the
  239.      * reference collision object inertial frame.
  240.      *
  241.      * @return other collision position and velocity relative to the reference collision object, expressed in the reference
  242.      * collision object inertial frame.
  243.      */
  244.     public FieldPVCoordinates<T> computeOtherRelativeToReferencePVInReferenceInertial() {

  245.         // Extract reference inertial frame
  246.         final Frame referenceInertial = referenceAtTCA.getFrame();

  247.         // Get PVCoordinates in the same frame
  248.         final FieldPVCoordinates<T> referencePV                = referenceAtTCA.getPVCoordinates();
  249.         final FieldKinematicTransform<T> kinematicTransform    = otherAtTCA.getFrame()
  250.             .getKinematicTransformTo(referenceInertial, referenceAtTCA.getDate());
  251.         final FieldPVCoordinates<T> otherPVInReferenceInertial = kinematicTransform
  252.             .transformOnlyPV(otherAtTCA.getPVCoordinates());

  253.         // Create relative pv expressed in the reference inertial frame
  254.         final FieldVector3D<T> relativePosition = otherPVInReferenceInertial.getPosition()
  255.                                                                             .subtract(referencePV.getPosition());
  256.         final FieldVector3D<T> relativeVelocity = otherPVInReferenceInertial.getVelocity()
  257.                                                                             .subtract(referencePV.getVelocity());

  258.         return new FieldPVCoordinates<>(relativePosition, relativeVelocity);
  259.     }

  260.     /**
  261.      * Compute the projection matrix from the reference collision object inertial frame to the collision plane.
  262.      * <p>
  263.      * Note that this matrix will only rotate from the reference collision object inertial frame to the encounter frame and
  264.      * project onto the collision plane, this is only a rotation.
  265.      * </p>
  266.      *
  267.      * @return projection matrix from the reference collision object inertial frame to the collision plane
  268.      */
  269.     public FieldMatrix<T> computeReferenceInertialToCollisionPlaneProjectionMatrix() {

  270.         // Create transform from reference inertial frame to encounter local orbital frame
  271.         final FieldStaticTransform<T> referenceInertialToEncounterFrameTransform =
  272.                 FieldStaticTransform.compose(tca,
  273.                                      computeReferenceInertialToReferenceTNWTransform(),
  274.                                      computeReferenceTNWToEncounterFrameTransform());

  275.         // Create rotation matrix from reference inertial frame to encounter local orbital frame
  276.         final FieldMatrix<T> referenceInertialToEncounterFrameRotationMatrix =
  277.                 new Array2DRowFieldMatrix<>(referenceInertialToEncounterFrameTransform.getRotation().getMatrix());

  278.         // Create projection matrix from encounter frame to collision plane
  279.         final FieldMatrix<T> encounterFrameToCollisionPlaneProjectionMatrix =
  280.                 encounterFrame.computeProjectionMatrix(tca.getField());

  281.         // Create projection matrix from reference inertial frame to collision plane
  282.         return encounterFrameToCollisionPlaneProjectionMatrix.multiply(referenceInertialToEncounterFrameRotationMatrix);
  283.     }

  284.     /**
  285.      * Compute the combined covariance matrix diagonalized and projected onto the collision plane.
  286.      * <p>
  287.      * Diagonalize projected positional covariance matrix in a specific manner to have
  288.      * <var>&#963;<sub>xx</sub><sup>2</sup> &#8804; &#963;<sub>yy</sub><sup>2</sup></var>.
  289.      *
  290.      * @return combined covariance matrix diagonalized and projected onto the collision plane
  291.      */
  292.     public FieldMatrix<T> computeProjectedAndDiagonalizedCombinedPositionalCovarianceMatrix() {

  293.         final FieldMatrix<T> covarianceMatrixToDiagonalize = computeProjectedCombinedPositionalCovarianceMatrix();

  294.         final T sigmaXSquared = covarianceMatrixToDiagonalize.getEntry(0, 0);
  295.         final T sigmaYSquared = covarianceMatrixToDiagonalize.getEntry(1, 1);

  296.         final T crossTerm = covarianceMatrixToDiagonalize.getEntry(0, 1);
  297.         final T recurrentTerm = sigmaXSquared.subtract(sigmaYSquared).multiply(0.5).pow(2)
  298.                                              .add(crossTerm.square()).sqrt();

  299.         final T eigenValueX = sigmaXSquared.add(sigmaYSquared).multiply(0.5).subtract(recurrentTerm);
  300.         final T eigenValueY = sigmaXSquared.add(sigmaYSquared).multiply(0.5).add(recurrentTerm);

  301.         final FieldMatrix<T> projectedAndDiagonalizedCombinedPositionalCovarianceMatrix =
  302.                 new BlockFieldMatrix<>(instanceField, 2, 2);
  303.         projectedAndDiagonalizedCombinedPositionalCovarianceMatrix.setEntry(0, 0, eigenValueX);
  304.         projectedAndDiagonalizedCombinedPositionalCovarianceMatrix.setEntry(0, 1, instanceField.getZero());
  305.         projectedAndDiagonalizedCombinedPositionalCovarianceMatrix.setEntry(1, 0, instanceField.getZero());
  306.         projectedAndDiagonalizedCombinedPositionalCovarianceMatrix.setEntry(1, 1, eigenValueY);

  307.         return projectedAndDiagonalizedCombinedPositionalCovarianceMatrix;
  308.     }

  309.     /**
  310.      * Compute the projected combined covariance matrix onto the collision plane.
  311.      *
  312.      * @return projected combined covariance matrix onto the collision plane
  313.      */
  314.     public FieldMatrix<T> computeProjectedCombinedPositionalCovarianceMatrix() {

  315.         // Compute the positional covariance in the encounter local orbital frame
  316.         final FieldMatrix<T> combinedPositionalCovarianceMatrixInEncounterFrame =
  317.                 computeCombinedCovarianceInEncounterFrame().getMatrix().getSubMatrix(0, 2, 0, 2);

  318.         // Project it onto the collision plane
  319.         return encounterFrame.projectOntoCollisionPlane(combinedPositionalCovarianceMatrixInEncounterFrame);
  320.     }

  321.     /**
  322.      * Compute the combined covariance expressed in the encounter frame.
  323.      *
  324.      * @return combined covariance expressed in the encounter frame
  325.      */
  326.     public FieldStateCovariance<T> computeCombinedCovarianceInEncounterFrame() {
  327.         return computeCombinedCovarianceInReferenceTNW().changeCovarianceFrame(referenceAtTCA, encounterFrame);
  328.     }

  329.     /**
  330.      * Compute the other collision object {@link FieldVector2D position} projected onto the collision plane.
  331.      *
  332.      * @return other collision object position projected onto the collision plane
  333.      */
  334.     public FieldVector2D<T> computeOtherPositionInCollisionPlane() {

  335.         // Express other in reference inertial
  336.         final FieldVector3D<T> otherInReferenceInertial = otherAtTCA.getPosition(referenceAtTCA.getFrame());

  337.         // Express other in reference TNW local orbital frame
  338.         final FieldVector3D<T> otherPositionInReferenceTNW =
  339.                 computeReferenceInertialToReferenceTNWTransform().transformPosition(otherInReferenceInertial);

  340.         // Express other in encounter local orbital frame
  341.         final FieldVector3D<T> otherPositionInEncounterFrame =
  342.                 computeReferenceTNWToEncounterFrameTransform().transformPosition(otherPositionInReferenceTNW);

  343.         return encounterFrame.projectOntoCollisionPlane(otherPositionInEncounterFrame);

  344.     }

  345.     /**
  346.      * Compute the other collision object {@link FieldVector2D position} in the rotated collision plane.
  347.      * <p>
  348.      * Uses a default zero threshold of 1e-15.
  349.      * <p>
  350.      * The coordinates are often noted xm and ym in probability of collision related papers.
  351.      * </p>
  352.      * <p>
  353.      * The mentioned rotation concerns the rotation that diagonalize the combined covariance matrix inside the collision
  354.      * plane.
  355.      * </p>
  356.      *
  357.      * @return other collision object position in the rotated collision plane
  358.      */
  359.     public FieldVector2D<T> computeOtherPositionInRotatedCollisionPlane() {
  360.         return computeOtherPositionInRotatedCollisionPlane(DEFAULT_ZERO_THRESHOLD);

  361.     }

  362.     /**
  363.      * Compute the other collision object {@link Vector2D position}  in the rotated collision plane.
  364.      * <p>
  365.      * The coordinates are often noted xm and ym in probability of collision related papers.
  366.      * <p>
  367.      * The mentioned rotation concerns the rotation that diagonalize the combined covariance matrix inside the collision
  368.      * plane.
  369.      *
  370.      * @param zeroThreshold threshold below which values are considered equal to zero
  371.      *
  372.      * @return other collision object position in the rotated collision plane
  373.      */
  374.     public FieldVector2D<T> computeOtherPositionInRotatedCollisionPlane(final double zeroThreshold) {

  375.         // Project the other position onto the collision plane
  376.         final FieldMatrix<T> otherPositionInCollisionPlaneMatrix =
  377.                 new Array2DRowFieldMatrix<>(computeOtherPositionInCollisionPlane().toArray());

  378.         // Express other in the rotated collision plane
  379.         final FieldMatrix<T> otherPositionRotatedInCollisionPlane =
  380.                 computeEncounterPlaneRotationMatrix(zeroThreshold).multiply(otherPositionInCollisionPlaneMatrix);

  381.         return new FieldVector2D<>(otherPositionRotatedInCollisionPlane.getColumn(0));

  382.     }

  383.     /**
  384.      * Compute the Encounter duration (s) evaluated using Coppola's formula described in : "COPPOLA, Vincent, et al.
  385.      * Evaluating the short encounter assumption of the probability of collision formula. 2012."
  386.      * <p>
  387.      * This method is to be used to check the validity of the short-term encounter model. The user is expected to compare the
  388.      * computed duration with the orbital period from both objects and draw its own conclusions.
  389.      * <p>
  390.      * It uses γ = 1e-16 as the resolution of a double is nearly 1e-16 so γ smaller than that are not meaningful to compute.
  391.      *
  392.      * @return encounter duration (s) evaluated using Coppola's formula
  393.      */
  394.     public T computeCoppolaEncounterDuration() {

  395.         // Default value for γ = 1e-16
  396.         final T DEFAULT_ALPHA_C = instanceField.getZero().newInstance(5.864);

  397.         final FieldMatrix<T> combinedPositionalCovarianceMatrix = computeCombinedCovarianceInEncounterFrame()
  398.                 .getMatrix().getSubMatrix(0, 2, 0, 2);

  399.         // Extract off-plane cross-term matrix
  400.         final FieldMatrix<T> projectionMatrix = encounterFrame.computeProjectionMatrix(instanceField);
  401.         final FieldMatrix<T> axisNormalToCollisionPlane =
  402.                 new Array2DRowFieldMatrix<>(encounterFrame.getAxisNormalToCollisionPlane(instanceField).toArray());
  403.         final FieldMatrix<T> offPlaneCrossTermMatrix =
  404.                 projectionMatrix.multiply(combinedPositionalCovarianceMatrix.multiply(axisNormalToCollisionPlane));

  405.         // Covariance sub-matrix of the in-plane terms
  406.         final FieldMatrix<T> probabilityDensity =
  407.                 encounterFrame.projectOntoCollisionPlane(combinedPositionalCovarianceMatrix);
  408.         final FieldMatrix<T> probabilityDensityInverse =
  409.                 new FieldLUDecomposition<>(probabilityDensity).getSolver().getInverse();

  410.         // Recurrent term in Coppola's paper : bᵀb
  411.         final FieldMatrix<T> b = offPlaneCrossTermMatrix.transposeMultiply(probabilityDensityInverse).transpose();
  412.         final T recurrentTerm = b.multiplyTransposed(b).getEntry(0, 0);

  413.         // Position uncertainty normal to collision plane
  414.         final T sigmaSqNormalToPlan = axisNormalToCollisionPlane.transposeMultiply(
  415.                 combinedPositionalCovarianceMatrix.multiply(axisNormalToCollisionPlane)).getEntry(0, 0);
  416.         final T sigmaV = sigmaSqNormalToPlan.subtract(b.multiplyTransposed(offPlaneCrossTermMatrix).getEntry(0, 0))
  417.                                             .sqrt();

  418.         final T relativeVelocity = computeOtherRelativeToReferencePVInReferenceInertial().getVelocity().getNorm();

  419.         return DEFAULT_ALPHA_C.multiply(sigmaV).multiply(2 * FastMath.sqrt(2)).add(
  420.                 combinedRadius.multiply(recurrentTerm.add(1).sqrt().add(recurrentTerm.sqrt()))).divide(relativeVelocity);
  421.     }

  422.     /**
  423.      * Compute the miss distance at time of closest approach.
  424.      *
  425.      * @return miss distance
  426.      */
  427.     public T computeMissDistance() {

  428.         // Get positions expressed in the same frame at time of closest approach
  429.         final FieldVector3D<T> referencePositionAtTCA = referenceAtTCA.getPosition();
  430.         final FieldVector3D<T> otherPositionAtTCA     = otherAtTCA.getPosition(referenceAtTCA.getFrame());

  431.         // Compute relative position
  432.         final FieldVector3D<T> relativePosition = otherPositionAtTCA.subtract(referencePositionAtTCA);

  433.         return relativePosition.getNorm();
  434.     }

  435.     /**
  436.      * Compute the Mahalanobis distance computed with the other collision object projected onto the collision plane (commonly
  437.      * called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix is
  438.      * diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
  439.      * <p>
  440.      * Uses a default zero threshold of 1e-15 for the computation of the diagonalizing of the projected covariance matrix.
  441.      *
  442.      * @return Mahalanobis distance between the reference and other collision object
  443.      *
  444.      * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
  445.      */
  446.     public T computeMahalanobisDistance() {
  447.         return computeMahalanobisDistance(DEFAULT_ZERO_THRESHOLD);
  448.     }

  449.     /**
  450.      * Compute the Mahalanobis distance computed with the other collision object projected onto the collision plane (commonly
  451.      * called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix is
  452.      * diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
  453.      *
  454.      * @param zeroThreshold threshold below which values are considered equal to zero
  455.      *
  456.      * @return Mahalanobis distance between the reference and other collision object
  457.      *
  458.      * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
  459.      */
  460.     public T computeMahalanobisDistance(final double zeroThreshold) {
  461.         return computeSquaredMahalanobisDistance(zeroThreshold).sqrt();
  462.     }

  463.     /**
  464.      * Compute the squared Mahalanobis distance computed with the other collision object projected onto the collision plane
  465.      * (commonly called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix
  466.      * is diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
  467.      * <p>
  468.      * Uses a default zero threshold of 1e-15 for the computation of the diagonalizing of the projected covariance matrix.
  469.      *
  470.      * @return squared Mahalanobis distance between the reference and other collision object
  471.      *
  472.      * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
  473.      */
  474.     public T computeSquaredMahalanobisDistance() {
  475.         return computeSquaredMahalanobisDistance(DEFAULT_ZERO_THRESHOLD);
  476.     }

  477.     /**
  478.      * Compute the squared Mahalanobis distance computed with the other collision object projected onto the collision plane
  479.      * (commonly called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix
  480.      * is diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
  481.      *
  482.      * @param zeroThreshold threshold below which values are considered equal to zero
  483.      *
  484.      * @return squared Mahalanobis distance between the reference and other collision object
  485.      *
  486.      * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
  487.      */
  488.     public T computeSquaredMahalanobisDistance(final double zeroThreshold) {

  489.         final FieldMatrix<T> otherPositionAfterRotationInCollisionPlane =
  490.                 new Array2DRowFieldMatrix<>(computeOtherPositionInRotatedCollisionPlane(zeroThreshold).toArray());

  491.         final FieldMatrix<T> inverseCovarianceMatrix =
  492.                 new FieldLUDecomposition<>(
  493.                         computeProjectedAndDiagonalizedCombinedPositionalCovarianceMatrix()).getSolver()
  494.                                                                                             .getInverse();

  495.         return otherPositionAfterRotationInCollisionPlane.transpose().multiply(
  496.                                                                  inverseCovarianceMatrix.multiply(
  497.                                                                          otherPositionAfterRotationInCollisionPlane))
  498.                                                          .getEntry(0, 0);
  499.     }

  500.     /** Get new encounter instance.
  501.      * @return new encounter instance
  502.      */
  503.     public ShortTermEncounter2DDefinition toEncounter() {
  504.         return new ShortTermEncounter2DDefinition(referenceAtTCA.toOrbit(), referenceCovariance.toStateCovariance(),
  505.                                                   otherAtTCA.toOrbit(), otherCovariance.toStateCovariance(),
  506.                                                   combinedRadius.getReal());
  507.     }

  508.     /**
  509.      * Takes both covariance matrices (expressed in their respective RTN local orbital frame) from reference and other
  510.      * collision object with which this instance was created and sum them in the reference collision object TNW local orbital
  511.      * frame.
  512.      *
  513.      * @return combined covariance matrix expressed in the reference collision object TNW local orbital frame
  514.      */
  515.     public FieldStateCovariance<T> computeCombinedCovarianceInReferenceTNW() {

  516.         // Express reference covariance in reference TNW local orbital frame
  517.         final FieldMatrix<T> referenceCovarianceMatrixInTNW =
  518.                 referenceCovariance.changeCovarianceFrame(referenceAtTCA, LOFType.TNW_INERTIAL).getMatrix();

  519.         // Express other covariance in reference inertial frame
  520.         final FieldMatrix<T> otherCovarianceMatrixInReferenceInertial =
  521.                 otherCovariance.changeCovarianceFrame(otherAtTCA, referenceAtTCA.getFrame()).getMatrix();

  522.         final FieldStateCovariance<T> otherCovarianceInReferenceInertial = new FieldStateCovariance<>(
  523.                 otherCovarianceMatrixInReferenceInertial, tca, referenceAtTCA.getFrame(),
  524.                 OrbitType.CARTESIAN, PositionAngleType.MEAN);

  525.         // Express other covariance in reference TNW local orbital frame
  526.         final FieldMatrix<T> otherCovarianceMatrixInReferenceTNW = otherCovarianceInReferenceInertial.changeCovarianceFrame(
  527.                 referenceAtTCA, LOFType.TNW_INERTIAL).getMatrix();

  528.         // Return the combined covariance expressed in the reference TNW local orbital frame
  529.         return new FieldStateCovariance<>(referenceCovarianceMatrixInTNW.add(otherCovarianceMatrixInReferenceTNW), tca,
  530.                                           LOFType.TNW_INERTIAL);
  531.     }

  532.     /**
  533.      * Compute the {@link FieldTransform transform} from the reference collision object inertial frame of reference to its
  534.      * TNW local orbital frame.
  535.      *
  536.      * @return transform from the reference collision object inertial frame of reference to its TNW local orbital frame
  537.      */
  538.     private FieldTransform<T> computeReferenceInertialToReferenceTNWTransform() {
  539.         return LOFType.TNW.transformFromInertial(tca, referenceAtTCA.getPVCoordinates());
  540.     }

  541.     /**
  542.      * Compute the {@link FieldTransform transform} from the reference collision object TNW local orbital frame to the
  543.      * encounter frame.
  544.      *
  545.      * @return transform from the reference collision object TNW local orbital frame to the encounter frame
  546.      */
  547.     private FieldTransform<T> computeReferenceTNWToEncounterFrameTransform() {
  548.         return LOF.transformFromLOFInToLOFOut(LOFType.TNW_INERTIAL, encounterFrame, tca,
  549.                                               referenceAtTCA.getPVCoordinates());
  550.     }

  551.     /**
  552.      * Compute the rotation matrix that diagonalize the combined positional covariance matrix projected onto the collision
  553.      * plane.
  554.      *
  555.      * @param zeroThreshold threshold below which values are considered equal to zero
  556.      *
  557.      * @return rotation matrix that diagonalize the combined covariance matrix projected onto the collision plane
  558.      */
  559.     private FieldMatrix<T> computeEncounterPlaneRotationMatrix(final double zeroThreshold) {

  560.         final FieldMatrix<T> combinedCovarianceMatrixInEncounterFrame =
  561.                 computeCombinedCovarianceInEncounterFrame().getMatrix();

  562.         final FieldMatrix<T> combinedPositionalCovarianceMatrixProjectedOntoBPlane =
  563.                 encounterFrame.projectOntoCollisionPlane(
  564.                         combinedCovarianceMatrixInEncounterFrame.getSubMatrix(0, 2, 0, 2));

  565.         final T sigmaXSquared = combinedPositionalCovarianceMatrixProjectedOntoBPlane.getEntry(0, 0);
  566.         final T sigmaYSquared = combinedPositionalCovarianceMatrixProjectedOntoBPlane.getEntry(1, 1);
  567.         final T crossTerm     = combinedPositionalCovarianceMatrixProjectedOntoBPlane.getEntry(0, 1);
  568.         final T correlation   = crossTerm.divide(sigmaXSquared.multiply(sigmaYSquared).sqrt());

  569.         // If the matrix is not initially diagonalized
  570.         final T theta;
  571.         if (FastMath.abs(crossTerm).getReal() > zeroThreshold) {
  572.             final T recurrentTerm = sigmaYSquared.subtract(sigmaXSquared).divide(crossTerm.multiply(2));
  573.             theta = recurrentTerm.subtract(correlation.sign().multiply(recurrentTerm.pow(2).add(1).sqrt())).atan();
  574.         }
  575.         // Else, the matrix is already diagonalized
  576.         else {
  577.             // Rotation in order to have sigmaXSquared < sigmaYSquared
  578.             if (sigmaXSquared.subtract(sigmaYSquared).getReal() > 0) {
  579.                 theta = tca.getField().getZero().newInstance(MathUtils.SEMI_PI);
  580.             }
  581.             // Else, there is no need for a rotation
  582.             else {
  583.                 theta = tca.getField().getZero();
  584.             }
  585.         }

  586.         final T                        cosTheta       = theta.cos();
  587.         final T                        sinTheta       = theta.sin();
  588.         final Array2DRowFieldMatrix<T> rotationMatrix = new Array2DRowFieldMatrix<>(tca.getField(), 2, 2);
  589.         rotationMatrix.setEntry(0, 0, cosTheta);
  590.         rotationMatrix.setEntry(0, 1, sinTheta);
  591.         rotationMatrix.setEntry(1, 0, sinTheta.negate());
  592.         rotationMatrix.setEntry(1, 1, cosTheta);

  593.         return rotationMatrix;
  594.     }

  595.     /**
  596.      * Get the Time of Closest Approach.
  597.      * <p>
  598.      * Commonly called TCA.
  599.      *
  600.      * @return time of closest approach
  601.      */
  602.     public FieldAbsoluteDate<T> getTca() {
  603.         return tca;
  604.     }

  605.     /** Get reference's orbit at time of closest approach.
  606.      * @return reference's orbit at time of closest approach
  607.      */
  608.     public FieldOrbit<T> getReferenceAtTCA() {
  609.         return referenceAtTCA;
  610.     }

  611.     /** Get other's orbit at time of closest approach.
  612.      *  @return other's orbit at time of closest approach
  613.      */
  614.     public FieldOrbit<T> getOtherAtTCA() {
  615.         return otherAtTCA;
  616.     }

  617.     /** Get reference's covariance.
  618.      * @return reference's covariance
  619.      */
  620.     public FieldStateCovariance<T> getReferenceCovariance() {
  621.         return referenceCovariance;
  622.     }

  623.     /** Get other's covariance.
  624.      * @return other's covariance
  625.      */
  626.     public FieldStateCovariance<T> getOtherCovariance() {
  627.         return otherCovariance;
  628.     }

  629.     /** Get combined radius.
  630.      * @return combined radius (m)
  631.      */
  632.     public T getCombinedRadius() {
  633.         return combinedRadius;
  634.     }

  635.     /** Get encounter local orbital frame.
  636.      * @return encounter local orbital frame
  637.      */
  638.     public EncounterLOF getEncounterFrame() {
  639.         return encounterFrame;
  640.     }

  641. }