ShortTermEncounter2DDefinition.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.geometry.euclidean.threed.Vector3D;
  19. import org.hipparchus.geometry.euclidean.twod.Vector2D;
  20. import org.hipparchus.linear.Array2DRowRealMatrix;
  21. import org.hipparchus.linear.EigenDecompositionSymmetric;
  22. import org.hipparchus.linear.LUDecomposition;
  23. import org.hipparchus.linear.RealMatrix;
  24. import org.hipparchus.util.FastMath;
  25. import org.hipparchus.util.MathUtils;
  26. import org.orekit.errors.OrekitException;
  27. import org.orekit.errors.OrekitMessages;
  28. import org.orekit.frames.Frame;
  29. import org.orekit.frames.LOF;
  30. import org.orekit.frames.LOFType;
  31. import org.orekit.frames.KinematicTransform;
  32. import org.orekit.frames.StaticTransform;
  33. import org.orekit.frames.Transform;
  34. import org.orekit.frames.encounter.EncounterLOF;
  35. import org.orekit.frames.encounter.EncounterLOFType;
  36. import org.orekit.orbits.Orbit;
  37. import org.orekit.orbits.OrbitType;
  38. import org.orekit.orbits.PositionAngleType;
  39. import org.orekit.propagation.StateCovariance;
  40. import org.orekit.time.AbsoluteDate;
  41. import org.orekit.utils.PVCoordinates;

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

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

  72.     /** Default epsilon when checking covariance matrix symmetry. */
  73.     private static final double DEFAULT_SYMMETRY_EPSILON = 1e-8;

  74.     /**
  75.      * Time of closest approach.
  76.      * <p>
  77.      * Commonly called TCA.
  78.      */
  79.     private final AbsoluteDate tca;

  80.     /** Reference collision object at time of closest approach. */
  81.     private final Orbit referenceAtTCA;

  82.     /** Reference collision object covariance matrix in its respective RTN frame. */
  83.     private final StateCovariance referenceCovariance;

  84.     /** Other collision object at time of closest approach. */
  85.     private final Orbit otherAtTCA;

  86.     /** Other collision object covariance matrix in its respective RTN frame. */
  87.     private final StateCovariance otherCovariance;

  88.     /** Combined radius (m). */
  89.     private final double combinedRadius;

  90.     /** Encounter local orbital frame to use. */
  91.     private final EncounterLOF encounterFrame;

  92.     /**
  93.      * Constructor.
  94.      *
  95.      * @param referenceAtTCA reference collision object orbit at time of closest approach
  96.      * @param referenceCovariance reference collision object covariance matrix in its respective RTN frame
  97.      * @param referenceRadius reference collision's equivalent sphere radius
  98.      * @param otherAtTCA other collision object  orbit at time of closest approach
  99.      * @param otherCovariance other collision object covariance matrix in its respective RTN frame
  100.      * @param otherRadius other collision's equivalent sphere radius
  101.      *
  102.      * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
  103.      */
  104.     public ShortTermEncounter2DDefinition(final Orbit referenceAtTCA, final StateCovariance referenceCovariance,
  105.                                           final double referenceRadius, final Orbit otherAtTCA,
  106.                                           final StateCovariance otherCovariance, final double otherRadius) {
  107.         this(referenceAtTCA, referenceCovariance, otherAtTCA, otherCovariance, referenceRadius + otherRadius);
  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 otherAtTCA other collision object  orbit at time of closest approach
  115.      * @param otherCovariance other collision object covariance matrix in its respective RTN frame
  116.      * @param combinedRadius combined radius (m)
  117.      *
  118.      * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
  119.      */
  120.     public ShortTermEncounter2DDefinition(final Orbit referenceAtTCA, final StateCovariance referenceCovariance,
  121.                                           final Orbit otherAtTCA, final StateCovariance otherCovariance,
  122.                                           final double combinedRadius) {
  123.         this(referenceAtTCA, referenceCovariance, otherAtTCA, otherCovariance, combinedRadius, EncounterLOFType.DEFAULT,
  124.              1e-6);
  125.     }

  126.     /**
  127.      * Constructor.
  128.      *
  129.      * @param referenceAtTCA reference collision object orbit at time of closest approach
  130.      * @param referenceCovariance reference collision object covariance matrix in its respective RTN frame
  131.      * @param referenceRadius reference collision's equivalent sphere radius
  132.      * @param otherAtTCA other collision object  orbit at time of closest approach
  133.      * @param otherCovariance other collision object covariance matrix in its respective RTN frame
  134.      * @param otherRadius other collision's equivalent sphere radius
  135.      * @param encounterFrameType type of encounter frame to use
  136.      * @param tcaTolerance tolerance on reference and other times of closest approach difference
  137.      *
  138.      * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
  139.      */
  140.     public ShortTermEncounter2DDefinition(final Orbit referenceAtTCA, final StateCovariance referenceCovariance,
  141.                                           final double referenceRadius, final Orbit otherAtTCA,
  142.                                           final StateCovariance otherCovariance, final double otherRadius,
  143.                                           final EncounterLOFType encounterFrameType, final double tcaTolerance) {
  144.         this(referenceAtTCA, referenceCovariance, otherAtTCA, otherCovariance, referenceRadius + otherRadius,
  145.              encounterFrameType, tcaTolerance);
  146.     }

  147.     /**
  148.      * Constructor.
  149.      *
  150.      * @param referenceAtTCA reference collision object orbit at time of closest approach
  151.      * @param referenceCovariance reference collision object covariance matrix in its respective RTN frame
  152.      * @param otherAtTCA other collision object  orbit at time of closest approach
  153.      * @param otherCovariance other collision object covariance matrix in its respective RTN frame
  154.      * @param combinedRadius combined radius (m)
  155.      * @param encounterFrameType type of encounter frame to use
  156.      * @param tcaTolerance tolerance on reference and other times of closest approach difference
  157.      *
  158.      * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
  159.      */
  160.     public ShortTermEncounter2DDefinition(final Orbit referenceAtTCA, final StateCovariance referenceCovariance,
  161.                                           final Orbit otherAtTCA, final StateCovariance otherCovariance,
  162.                                           final double combinedRadius, final EncounterLOFType encounterFrameType,
  163.                                           final double tcaTolerance) {

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

  165.             this.tca = referenceAtTCA.getDate();

  166.             this.referenceAtTCA      = referenceAtTCA;
  167.             this.referenceCovariance = referenceCovariance;

  168.             this.otherAtTCA      = otherAtTCA;
  169.             this.otherCovariance = otherCovariance;

  170.             this.combinedRadius = combinedRadius;

  171.             this.encounterFrame = encounterFrameType.getFrame(otherAtTCA.getPVCoordinates());
  172.         } else {
  173.             throw new OrekitException(OrekitMessages.DIFFERENT_TIME_OF_CLOSEST_APPROACH);
  174.         }

  175.     }

  176.     /**
  177.      * Compute the squared Mahalanobis distance.
  178.      *
  179.      * @param xm other collision object projected xm position onto the collision plane in the rotated encounter frame
  180.      * @param ym other collision object projected ym position onto the collision plane in the rotated encounter frame
  181.      * @param sigmaX square root of the x-axis eigen value of the diagonalized combined covariance matrix projected onto the
  182.      * collision plane
  183.      * @param sigmaY square root of the y-axis eigen value of the diagonalized combined covariance matrix projected onto the
  184.      * collision plane
  185.      *
  186.      * @return squared Mahalanobis distance
  187.      */
  188.     public static double computeSquaredMahalanobisDistance(final double xm, final double ym,
  189.                                                            final double sigmaX, final double sigmaY) {
  190.         final Vector2D position = new Vector2D(xm, ym);

  191.         final RealMatrix covariance = new Array2DRowRealMatrix(new double[][] {
  192.                 { sigmaX * sigmaX, 0 },
  193.                 { 0, sigmaY * sigmaY } });

  194.         return computeSquaredMahalanobisDistance(position, covariance);
  195.     }

  196.     /**
  197.      * Compute the squared Mahalanobis distance.
  198.      *
  199.      * @param otherPosition other collision object projected position onto the collision plane in the rotated encounter
  200.      * frame
  201.      * @param covarianceMatrix combined covariance matrix projected onto the collision plane and diagonalized
  202.      *
  203.      * @return squared Mahalanobis distance
  204.      */
  205.     public static double computeSquaredMahalanobisDistance(final Vector2D otherPosition, final RealMatrix covarianceMatrix) {

  206.         final RealMatrix covarianceMatrixInverse = new LUDecomposition(covarianceMatrix).getSolver().getInverse();

  207.         final RealMatrix otherPositionOnCollisionPlaneMatrix = new Array2DRowRealMatrix(otherPosition.toArray());

  208.         return otherPositionOnCollisionPlaneMatrix.transposeMultiply(
  209.                 covarianceMatrixInverse.multiply(otherPositionOnCollisionPlaneMatrix)).getEntry(0, 0);
  210.     }

  211.     /**
  212.      * Compute the other collision position and velocity relative to the reference collision object. Expressed in the
  213.      * reference collision object inertial frame.
  214.      *
  215.      * @return other collision position and velocity relative to the reference collision object, expressed in the reference
  216.      * collision object inertial frame.
  217.      */
  218.     public PVCoordinates computeOtherRelativeToReferencePVInReferenceInertial() {

  219.         // Extract reference inertial frame
  220.         final Frame referenceInertial = referenceAtTCA.getFrame();

  221.         // Get PVCoordinates in the same frame
  222.         final PVCoordinates referencePV                = referenceAtTCA.getPVCoordinates();
  223.         final KinematicTransform kinematicTransform = otherAtTCA.getFrame().getKinematicTransformTo(referenceInertial,
  224.             otherAtTCA.getDate());
  225.         final PVCoordinates otherPVInReferenceInertial = kinematicTransform.transformOnlyPV(otherAtTCA.getPVCoordinates());

  226.         // Create relative pv expressed in the reference inertial frame
  227.         final Vector3D relativePosition = otherPVInReferenceInertial.getPosition().subtract(referencePV.getPosition());
  228.         final Vector3D relativeVelocity = otherPVInReferenceInertial.getVelocity().subtract(referencePV.getVelocity());

  229.         return new PVCoordinates(relativePosition, relativeVelocity);
  230.     }

  231.     /**
  232.      * Compute the projection matrix from the reference collision object inertial frame to the collision plane.
  233.      * <p>
  234.      * Note that this matrix will only rotate from the reference collision object inertial frame to the encounter frame and
  235.      * project onto the collision plane, this is only a rotation.
  236.      * </p>
  237.      *
  238.      * @return projection matrix from the reference collision object inertial frame to the collision plane
  239.      */
  240.     public RealMatrix computeReferenceInertialToCollisionPlaneProjectionMatrix() {

  241.         // Create transform from reference inertial frame to encounter local orbital frame
  242.         final StaticTransform referenceInertialToEncounterFrameTransform =
  243.                 StaticTransform.compose(tca, computeReferenceInertialToReferenceTNWTransform(),
  244.                               computeReferenceTNWToEncounterFrameTransform());

  245.         // Create rotation matrix from reference inertial frame to encounter local orbital frame
  246.         final RealMatrix referenceInertialToEncounterFrameRotationMatrix = new Array2DRowRealMatrix(
  247.                 referenceInertialToEncounterFrameTransform.getRotation().getMatrix());

  248.         // Create projection matrix from encounter frame to collision plane
  249.         final RealMatrix encounterFrameToCollisionPlaneProjectionMatrix = encounterFrame.computeProjectionMatrix();

  250.         // Create projection matrix from reference inertial frame to collision plane
  251.         return encounterFrameToCollisionPlaneProjectionMatrix.multiply(referenceInertialToEncounterFrameRotationMatrix);
  252.     }

  253.     /**
  254.      * Compute the combined covariance matrix diagonalized and projected onto the collision plane.
  255.      * <p>
  256.      * Diagonalize projected positional covariance matrix in a specific manner to have
  257.      * <var>&#963;<sub>xx</sub><sup>2</sup> &#8804; &#963;<sub>yy</sub><sup>2</sup></var>.
  258.      *
  259.      * @return combined covariance matrix diagonalized and projected onto the collision plane
  260.      */
  261.     public RealMatrix computeProjectedAndDiagonalizedCombinedPositionalCovarianceMatrix() {
  262.         final RealMatrix covariance = computeProjectedCombinedPositionalCovarianceMatrix();
  263.         final EigenDecompositionSymmetric ed = new EigenDecompositionSymmetric(covariance, DEFAULT_SYMMETRY_EPSILON, false);
  264.         return ed.getD();
  265.     }

  266.     /**
  267.      * Compute the projected combined covariance matrix onto the collision plane.
  268.      *
  269.      * @return projected combined covariance matrix onto the collision plane
  270.      */
  271.     public RealMatrix computeProjectedCombinedPositionalCovarianceMatrix() {

  272.         // Compute the positional covariance in the encounter local orbital frame
  273.         final RealMatrix combinedPositionalCovarianceMatrixInEncounterFrame =
  274.                 computeCombinedCovarianceInEncounterFrame().getMatrix().getSubMatrix(0, 2, 0, 2);

  275.         // Project it onto the collision plane
  276.         return encounterFrame.projectOntoCollisionPlane(combinedPositionalCovarianceMatrixInEncounterFrame);
  277.     }

  278.     /**
  279.      * Compute the combined covariance expressed in the encounter frame.
  280.      *
  281.      * @return combined covariance expressed in the encounter frame
  282.      */
  283.     public StateCovariance computeCombinedCovarianceInEncounterFrame() {
  284.         return computeCombinedCovarianceInReferenceTNW().changeCovarianceFrame(referenceAtTCA, encounterFrame);
  285.     }

  286.     /**
  287.      * Compute the other collision object {@link Vector2D position} projected onto the collision plane.
  288.      *
  289.      * @return other collision object position projected onto the collision plane
  290.      */
  291.     public Vector2D computeOtherPositionInCollisionPlane() {

  292.         // Express other in reference inertial
  293.         final Vector3D otherInReferenceInertial = otherAtTCA.getPosition(referenceAtTCA.getFrame());

  294.         // Express other in reference TNW local orbital frame
  295.         final Vector3D otherPositionInReferenceTNW =
  296.                 computeReferenceInertialToReferenceTNWTransform().transformPosition(otherInReferenceInertial);

  297.         // Express other in encounter local orbital frame
  298.         final Vector3D otherPositionInEncounterFrame =
  299.                 computeReferenceTNWToEncounterFrameTransform().transformPosition(otherPositionInReferenceTNW);

  300.         return encounterFrame.projectOntoCollisionPlane(otherPositionInEncounterFrame);

  301.     }

  302.     /**
  303.      * Compute the other collision object {@link Vector2D position} in the rotated collision plane.
  304.      * <p>
  305.      * Uses a default zero threshold of 1e-15.
  306.      * <p>
  307.      * The coordinates are often noted xm and ym in probability of collision related papers.
  308.      * </p>
  309.      * <p>
  310.      * The mentioned rotation concerns the rotation that diagonalize the combined covariance matrix inside the collision
  311.      * plane.
  312.      * </p>
  313.      *
  314.      * @return other collision object position in the rotated collision plane
  315.      */
  316.     public Vector2D computeOtherPositionInRotatedCollisionPlane() {
  317.         return computeOtherPositionInRotatedCollisionPlane(DEFAULT_ZERO_THRESHOLD);

  318.     }

  319.     /**
  320.      * Compute the other collision object {@link Vector2D position}  in the rotated collision plane.
  321.      * <p>
  322.      * The coordinates are often noted xm and ym in probability of collision related papers.
  323.      * <p>
  324.      * The mentioned rotation concerns the rotation that diagonalize the combined covariance matrix inside the collision
  325.      * plane.
  326.      *
  327.      * @param zeroThreshold threshold below which values are considered equal to zero
  328.      *
  329.      * @return other collision object position in the rotated collision plane
  330.      */
  331.     public Vector2D computeOtherPositionInRotatedCollisionPlane(final double zeroThreshold) {

  332.         // Project the other position onto the collision plane
  333.         final RealMatrix otherPositionInCollisionPlaneMatrix =
  334.                 new Array2DRowRealMatrix(computeOtherPositionInCollisionPlane().toArray());

  335.         // Express other in the rotated collision plane
  336.         final RealMatrix otherPositionRotatedInCollisionPlane =
  337.                 computeEncounterPlaneRotationMatrix(zeroThreshold).multiply(otherPositionInCollisionPlaneMatrix);

  338.         return new Vector2D(otherPositionRotatedInCollisionPlane.getColumn(0));

  339.     }

  340.     /**
  341.      * Compute the Encounter duration (s) evaluated using Coppola's formula described in : "COPPOLA, Vincent, et al.
  342.      * Evaluating the short encounter assumption of the probability of collision formula. 2012."
  343.      * <p>
  344.      * This method is to be used to check the validity of the short-term encounter model. The user is expected to compare the
  345.      * computed duration with the orbital period from both objects and draw its own conclusions.
  346.      * <p>
  347.      * It uses γ = 1e-16 as the resolution of a double is nearly 1e-16 so γ smaller than that are not meaningful to compute.
  348.      *
  349.      * @return encounter duration (s) evaluated using Coppola's formula
  350.      */
  351.     public double computeCoppolaEncounterDuration() {

  352.         // Default value for γ = 1e-16
  353.         final double DEFAULT_ALPHA_C = 5.864;

  354.         final RealMatrix combinedPositionalCovarianceMatrix = computeCombinedCovarianceInEncounterFrame()
  355.                 .getMatrix().getSubMatrix(0, 2, 0, 2);

  356.         // Extract off-plane cross-term matrix
  357.         final RealMatrix projectionMatrix = encounterFrame.computeProjectionMatrix();
  358.         final RealMatrix axisNormalToCollisionPlane =
  359.                 new Array2DRowRealMatrix(encounterFrame.getAxisNormalToCollisionPlane().toArray());
  360.         final RealMatrix offPlaneCrossTermMatrix =
  361.                 projectionMatrix.multiply(combinedPositionalCovarianceMatrix.multiply(axisNormalToCollisionPlane));

  362.         // Covariance sub-matrix of the in-plane terms
  363.         final RealMatrix probabilityDensity =
  364.                 encounterFrame.projectOntoCollisionPlane(combinedPositionalCovarianceMatrix);
  365.         final RealMatrix probabilityDensityInverse =
  366.                 new LUDecomposition(probabilityDensity).getSolver().getInverse();

  367.         // Recurrent term in Coppola's paper : bᵀb
  368.         final RealMatrix b             = offPlaneCrossTermMatrix.transposeMultiply(probabilityDensityInverse).transpose();
  369.         final double     recurrentTerm = b.multiplyTransposed(b).getEntry(0, 0);

  370.         // Position uncertainty normal to collision plane
  371.         final double sigmaSqNormalToPlan = axisNormalToCollisionPlane.transposeMultiply(
  372.                 combinedPositionalCovarianceMatrix.multiply(axisNormalToCollisionPlane)).getEntry(0, 0);
  373.         final double sigmaV = FastMath.sqrt(
  374.                 sigmaSqNormalToPlan - b.multiplyTransposed(offPlaneCrossTermMatrix).getEntry(0, 0));

  375.         final double relativeVelocity = computeOtherRelativeToReferencePVInReferenceInertial().getVelocity().getNorm();

  376.         return (2 * FastMath.sqrt(2) * DEFAULT_ALPHA_C * sigmaV + combinedRadius * (
  377.                 FastMath.sqrt(1 + recurrentTerm) + FastMath.sqrt(recurrentTerm))) / relativeVelocity;
  378.     }

  379.     /**
  380.      * Compute the miss distance at time of closest approach.
  381.      *
  382.      * @return miss distance
  383.      */
  384.     public double computeMissDistance() {

  385.         // Get positions expressed in the same frame at time of closest approach
  386.         final Vector3D referencePositionAtTCA = referenceAtTCA.getPosition();
  387.         final Vector3D otherPositionAtTCA     = otherAtTCA.getPosition(referenceAtTCA.getFrame());

  388.         // Compute relative position
  389.         final Vector3D relativePosition = otherPositionAtTCA.subtract(referencePositionAtTCA);

  390.         return relativePosition.getNorm();
  391.     }

  392.     /**
  393.      * Compute the Mahalanobis distance computed with the other collision object projected onto the collision plane (commonly
  394.      * called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix is
  395.      * diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
  396.      * <p>
  397.      * Uses a default zero threshold of 1e-15 for the computation of the diagonalizing of the projected covariance matrix.
  398.      *
  399.      * @return Mahalanobis distance between the reference and other collision object
  400.      *
  401.      * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
  402.      */
  403.     public double computeMahalanobisDistance() {
  404.         return computeMahalanobisDistance(DEFAULT_ZERO_THRESHOLD);
  405.     }

  406.     /**
  407.      * Compute the Mahalanobis distance computed with the other collision object projected onto the collision plane (commonly
  408.      * called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix is
  409.      * diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
  410.      *
  411.      * @param zeroThreshold threshold below which values are considered equal to zero
  412.      *
  413.      * @return Mahalanobis distance between the reference and other collision object
  414.      *
  415.      * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
  416.      */
  417.     public double computeMahalanobisDistance(final double zeroThreshold) {
  418.         return FastMath.sqrt(computeSquaredMahalanobisDistance(zeroThreshold));
  419.     }

  420.     /**
  421.      * Compute the squared Mahalanobis distance computed with the other collision object projected onto the collision plane
  422.      * (commonly called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix
  423.      * is diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
  424.      * <p>
  425.      * Uses a default zero threshold of 1e-15 for the computation of the diagonalizing of the projected covariance matrix.
  426.      *
  427.      * @return squared Mahalanobis distance between the reference and other collision object
  428.      *
  429.      * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
  430.      */
  431.     public double computeSquaredMahalanobisDistance() {
  432.         return computeSquaredMahalanobisDistance(DEFAULT_ZERO_THRESHOLD);
  433.     }

  434.     /**
  435.      * Compute the squared Mahalanobis distance computed with the other collision object projected onto the collision plane
  436.      * (commonly called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix
  437.      * is diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
  438.      *
  439.      * @param zeroThreshold threshold below which values are considered equal to zero
  440.      *
  441.      * @return squared Mahalanobis distance between the reference and other collision object
  442.      *
  443.      * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
  444.      */
  445.     public double computeSquaredMahalanobisDistance(final double zeroThreshold) {

  446.         final RealMatrix otherPositionAfterRotationInCollisionPlane =
  447.                 new Array2DRowRealMatrix(computeOtherPositionInRotatedCollisionPlane(zeroThreshold).toArray());

  448.         final RealMatrix inverseCovarianceMatrix =
  449.                 new LUDecomposition(computeProjectedAndDiagonalizedCombinedPositionalCovarianceMatrix()).getSolver()
  450.                                                                                                         .getInverse();

  451.         return otherPositionAfterRotationInCollisionPlane.transpose().multiply(
  452.                 inverseCovarianceMatrix.multiply(otherPositionAfterRotationInCollisionPlane)).getEntry(0, 0);
  453.     }

  454.     /**
  455.      * Takes both covariance matrices (expressed in their respective RTN local orbital frame) from reference and other
  456.      * collision object with which this instance was created and sum them in the reference collision object TNW local orbital
  457.      * frame.
  458.      *
  459.      * @return combined covariance matrix expressed in the reference collision object TNW local orbital frame
  460.      */
  461.     public StateCovariance computeCombinedCovarianceInReferenceTNW() {

  462.         // Express reference covariance in reference TNW local orbital frame
  463.         final RealMatrix referenceCovarianceMatrixInTNW =
  464.                 referenceCovariance.changeCovarianceFrame(referenceAtTCA, LOFType.TNW_INERTIAL).getMatrix();

  465.         // Express other covariance in reference inertial frame
  466.         final RealMatrix otherCovarianceMatrixInReferenceInertial =
  467.                 otherCovariance.changeCovarianceFrame(otherAtTCA, referenceAtTCA.getFrame()).getMatrix();

  468.         final StateCovariance otherCovarianceInReferenceInertial = new StateCovariance(
  469.                 otherCovarianceMatrixInReferenceInertial, tca, referenceAtTCA.getFrame(),
  470.                 OrbitType.CARTESIAN, PositionAngleType.MEAN);

  471.         // Express other covariance in reference TNW local orbital frame
  472.         final RealMatrix otherCovarianceMatrixInReferenceTNW = otherCovarianceInReferenceInertial.changeCovarianceFrame(
  473.                 referenceAtTCA, LOFType.TNW_INERTIAL).getMatrix();

  474.         // Return the combined covariance expressed in the reference TNW local orbital frame
  475.         return new StateCovariance(referenceCovarianceMatrixInTNW.add(otherCovarianceMatrixInReferenceTNW), tca,
  476.                                    LOFType.TNW_INERTIAL);
  477.     }

  478.     /**
  479.      * Compute the {@link Transform transform} from the reference collision object inertial frame of reference to its TNW
  480.      * local orbital frame.
  481.      *
  482.      * @return transform from the reference collision object inertial frame of reference to its TNW local orbital frame
  483.      */
  484.     private Transform computeReferenceInertialToReferenceTNWTransform() {
  485.         return LOFType.TNW.transformFromInertial(tca, referenceAtTCA.getPVCoordinates());
  486.     }

  487.     /**
  488.      * Compute the {@link Transform transform} from the reference collision object TNW local orbital frame to the encounter
  489.      * frame.
  490.      *
  491.      * @return transform from the reference collision object TNW local orbital frame to the encounter frame
  492.      */
  493.     private Transform computeReferenceTNWToEncounterFrameTransform() {
  494.         return LOF.transformFromLOFInToLOFOut(LOFType.TNW_INERTIAL, encounterFrame, tca,
  495.                                               referenceAtTCA.getPVCoordinates());
  496.     }

  497.     /**
  498.      * Compute the rotation matrix that diagonalize the combined positional covariance matrix projected onto the collision
  499.      * plane.
  500.      *
  501.      * @param zeroThreshold threshold below which values are considered equal to zero
  502.      *
  503.      * @return rotation matrix that diagonalize the combined covariance matrix projected onto the collision plane
  504.      */
  505.     private RealMatrix computeEncounterPlaneRotationMatrix(final double zeroThreshold) {

  506.         final RealMatrix combinedCovarianceMatrixInEncounterFrame =
  507.                 computeCombinedCovarianceInEncounterFrame().getMatrix();

  508.         final RealMatrix combinedPositionalCovarianceMatrixProjectedOntoBPlane =
  509.                 encounterFrame.projectOntoCollisionPlane(
  510.                         combinedCovarianceMatrixInEncounterFrame.getSubMatrix(0, 2, 0, 2));

  511.         final double sigmaXSquared = combinedPositionalCovarianceMatrixProjectedOntoBPlane.getEntry(0, 0);
  512.         final double sigmaYSquared = combinedPositionalCovarianceMatrixProjectedOntoBPlane.getEntry(1, 1);
  513.         final double crossTerm     = combinedPositionalCovarianceMatrixProjectedOntoBPlane.getEntry(0, 1);
  514.         final double correlation   = crossTerm / (FastMath.sqrt(sigmaXSquared * sigmaYSquared));

  515.         // If the matrix is not initially diagonalized
  516.         final double theta;
  517.         if (FastMath.abs(crossTerm) > zeroThreshold) {
  518.             final double recurrentTerm = (sigmaYSquared - sigmaXSquared) / (2 * crossTerm);
  519.             theta = FastMath.atan(
  520.                     recurrentTerm - FastMath.signum(correlation) * FastMath.sqrt(1 + FastMath.pow(recurrentTerm, 2)));
  521.         }
  522.         // Else, the matrix is already diagonalized
  523.         else {
  524.             // Rotation in order to have sigmaXSquared < sigmaYSquared
  525.             if (sigmaXSquared - sigmaYSquared > 0) {
  526.                 theta = MathUtils.SEMI_PI;
  527.             }
  528.             // Else, there is no need for a rotation
  529.             else {
  530.                 theta = 0;
  531.             }
  532.         }

  533.         final double[][] collisionPlaneRotationMatrixData = { { FastMath.cos(theta), FastMath.sin(theta) },
  534.                                                               { -FastMath.sin(theta), FastMath.cos(theta) } };

  535.         return new Array2DRowRealMatrix(collisionPlaneRotationMatrixData);
  536.     }

  537.     /**
  538.      * Get the Time of Closest Approach.
  539.      * <p>
  540.      * Commonly called TCA.
  541.      *
  542.      * @return time of closest approach
  543.      */
  544.     public AbsoluteDate getTca() {
  545.         return tca;
  546.     }

  547.     /** Get reference's orbit at time of closest approach.
  548.      * @return reference's orbit at time of closest approach
  549.      */
  550.     public Orbit getReferenceAtTCA() {
  551.         return referenceAtTCA;
  552.     }

  553.     /** Get other's orbit at time of closest approach.
  554.      *  @return other's orbit at time of closest approach
  555.      */
  556.     public Orbit getOtherAtTCA() {
  557.         return otherAtTCA;
  558.     }

  559.     /** Get reference's covariance.
  560.      * @return reference's covariance
  561.      */
  562.     public StateCovariance getReferenceCovariance() {
  563.         return referenceCovariance;
  564.     }

  565.     /** Get other's covariance.
  566.      * @return other's covariance
  567.      */
  568.     public StateCovariance getOtherCovariance() {
  569.         return otherCovariance;
  570.     }

  571.     /** Get combined radius.
  572.      * @return combined radius (m)
  573.      */
  574.     public double getCombinedRadius() {
  575.         return combinedRadius;
  576.     }

  577.     /** Get encounter local orbital frame.
  578.      * @return encounter local orbital frame
  579.      */
  580.     public EncounterLOF getEncounterFrame() {
  581.         return encounterFrame;
  582.     }

  583. }