FieldShortTermEncounter2DDefinition.java
- /* Copyright 2002-2024 CS GROUP
- * Licensed to CS GROUP (CS) under one or more
- * contributor license agreements. See the NOTICE file distributed with
- * this work for additional information regarding copyright ownership.
- * CS licenses this file to You under the Apache License, Version 2.0
- * (the "License"); you may not use this file except in compliance with
- * the License. You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
- package org.orekit.ssa.collision.shorttermencounter.probability.twod;
- import org.hipparchus.CalculusFieldElement;
- import org.hipparchus.Field;
- import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
- import org.hipparchus.geometry.euclidean.twod.FieldVector2D;
- import org.hipparchus.geometry.euclidean.twod.Vector2D;
- import org.hipparchus.linear.Array2DRowFieldMatrix;
- import org.hipparchus.linear.BlockFieldMatrix;
- import org.hipparchus.linear.FieldLUDecomposition;
- import org.hipparchus.linear.FieldMatrix;
- import org.hipparchus.util.FastMath;
- import org.hipparchus.util.MathArrays;
- import org.hipparchus.util.MathUtils;
- import org.orekit.errors.OrekitException;
- import org.orekit.errors.OrekitMessages;
- import org.orekit.frames.FieldKinematicTransform;
- import org.orekit.frames.FieldStaticTransform;
- import org.orekit.frames.FieldTransform;
- import org.orekit.frames.Frame;
- import org.orekit.frames.LOF;
- import org.orekit.frames.LOFType;
- import org.orekit.frames.encounter.EncounterLOF;
- import org.orekit.frames.encounter.EncounterLOFType;
- import org.orekit.orbits.FieldOrbit;
- import org.orekit.orbits.OrbitType;
- import org.orekit.orbits.PositionAngleType;
- import org.orekit.propagation.FieldStateCovariance;
- import org.orekit.time.FieldAbsoluteDate;
- import org.orekit.utils.FieldPVCoordinates;
- /**
- * Defines the encounter between two collision object at time of closest approach assuming a short-term encounter model . It
- * uses the given {@link EncounterLOFType encounter frame type} to define the encounter.
- * <p>
- * Both the primary and secondary collision object can be at the reference of the encounter frame, it is up to the user to
- * choose.
- * <p>
- * The "reference" object is the object considered at the reference of the given encounter frame while the "other" object is
- * the one <b>not placed</b> at the reference.
- * <p>
- * For example, if the user wants the primary to be at the reference of the default encounter frame, they will have to input
- * data in the following manner:
- * <pre>{@code
- * final FieldShortTermEncounter2DDefinition encounter = new FieldShortTermEncounter2DDefinition<>(primaryOrbitAtTCA, primaryCovarianceAtTCA, primaryRadius, secondaryOrbitAtTCA, secondaryCovarianceAtTCA, secondaryRadius);
- * }
- * </pre>
- * However, if the user wants to put the secondary at the reference and use the
- * {@link org.orekit.frames.encounter.ValsecchiEncounterFrame Valsecchi encounter frame}, they will have to type :
- * <pre>{@code
- * final FieldShortTermEncounter2DDefinition encounter = new FieldShortTermEncounter2DDefinition<>(secondaryOrbitAtTCA, secondaryCovarianceAtTCA, secondaryRadius, primaryOrbitAtTCA, primaryCovarianceAtTCA, primaryRadius, EncounterLOFType.VALSECCHI_2003);
- * }
- * </pre>
- * Note that in the current implementation, the shape of the collision objects is assumed to be a sphere.
- *
- * @author Vincent Cucchietti
- * @since 12.0
- * @param <T> type of the field elements
- */
- public class FieldShortTermEncounter2DDefinition<T extends CalculusFieldElement<T>> {
- /** Default threshold below which values are considered equal to zero. */
- private static final double DEFAULT_ZERO_THRESHOLD = 1e-15;
- /** Field to which the instance elements belong. */
- private final Field<T> instanceField;
- /**
- * Time of closest approach.
- * <p>
- * Commonly called TCA.
- */
- private final FieldAbsoluteDate<T> tca;
- /** Reference collision object at time of closest approach. */
- private final FieldOrbit<T> referenceAtTCA;
- /** Reference collision object covariance matrix in its respective RTN frame. */
- private final FieldStateCovariance<T> referenceCovariance;
- /** Other collision object at time of closest approach. */
- private final FieldOrbit<T> otherAtTCA;
- /** Other collision object covariance matrix in its respective RTN frame. */
- private final FieldStateCovariance<T> otherCovariance;
- /** Combined radius (m). */
- private final T combinedRadius;
- /** Encounter local orbital frame to use. */
- private final EncounterLOF encounterFrame;
- /**
- * Constructor.
- *
- * @param referenceAtTCA reference collision object orbit at time of closest approach
- * @param referenceCovariance reference collision object covariance matrix in its respective RTN frame
- * @param referenceRadius reference collision's equivalent sphere radius
- * @param otherAtTCA other collision object orbit at time of closest approach
- * @param otherCovariance other collision object covariance matrix in its respective RTN frame
- * @param otherRadius other collision's equivalent sphere radius
- *
- * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
- */
- public FieldShortTermEncounter2DDefinition(final FieldOrbit<T> referenceAtTCA,
- final FieldStateCovariance<T> referenceCovariance,
- final T referenceRadius,
- final FieldOrbit<T> otherAtTCA,
- final FieldStateCovariance<T> otherCovariance,
- final T otherRadius) {
- this(referenceAtTCA, referenceCovariance, otherAtTCA, otherCovariance, referenceRadius.add(otherRadius));
- }
- /**
- * Constructor.
- *
- * @param referenceAtTCA reference collision object orbit at time of closest approach
- * @param referenceCovariance reference collision object covariance matrix in its respective RTN frame
- * @param otherAtTCA other collision object orbit at time of closest approach
- * @param otherCovariance other collision object covariance matrix in its respective RTN frame
- * @param combinedRadius combined radius (m)
- *
- * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
- */
- public FieldShortTermEncounter2DDefinition(final FieldOrbit<T> referenceAtTCA,
- final FieldStateCovariance<T> referenceCovariance,
- final FieldOrbit<T> otherAtTCA,
- final FieldStateCovariance<T> otherCovariance,
- final T combinedRadius) {
- this(referenceAtTCA, referenceCovariance, otherAtTCA, otherCovariance, combinedRadius,
- EncounterLOFType.DEFAULT, 1e-6);
- }
- /**
- * Constructor.
- *
- * @param referenceAtTCA reference collision object orbit at time of closest approach
- * @param referenceCovariance reference collision object covariance matrix in its respective RTN frame
- * @param referenceRadius reference collision's equivalent sphere radius
- * @param otherAtTCA other collision object orbit at time of closest approach
- * @param otherCovariance other collision object covariance matrix in its respective RTN frame
- * @param otherRadius other collision's equivalent sphere radius
- * @param encounterFrameType type of encounter frame to use
- * @param tcaTolerance tolerance on reference and other times of closest approach difference
- *
- * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
- */
- public FieldShortTermEncounter2DDefinition(final FieldOrbit<T> referenceAtTCA,
- final FieldStateCovariance<T> referenceCovariance,
- final T referenceRadius,
- final FieldOrbit<T> otherAtTCA,
- final FieldStateCovariance<T> otherCovariance,
- final T otherRadius,
- final EncounterLOFType encounterFrameType,
- final double tcaTolerance) {
- this(referenceAtTCA, referenceCovariance, otherAtTCA, otherCovariance, referenceRadius.add(otherRadius),
- encounterFrameType, tcaTolerance);
- }
- /**
- * Constructor.
- *
- * @param referenceAtTCA reference collision object orbit at time of closest approach
- * @param referenceCovariance reference collision object covariance matrix in its respective RTN frame
- * @param otherAtTCA other collision object orbit at time of closest approach
- * @param otherCovariance other collision object covariance matrix in its respective RTN frame
- * @param combinedRadius combined radius (m)
- * @param encounterFrameType type of encounter frame to use
- * @param tcaTolerance tolerance on reference and other times of closest approach difference
- *
- * @throws OrekitException If both collision object spacecraft state don't have the same definition date.
- */
- public FieldShortTermEncounter2DDefinition(final FieldOrbit<T> referenceAtTCA,
- final FieldStateCovariance<T> referenceCovariance,
- final FieldOrbit<T> otherAtTCA,
- final FieldStateCovariance<T> otherCovariance,
- final T combinedRadius,
- final EncounterLOFType encounterFrameType,
- final double tcaTolerance) {
- if (referenceAtTCA.getDate().isCloseTo(otherAtTCA.getDate(), tcaTolerance)) {
- this.tca = referenceAtTCA.getDate();
- this.instanceField = tca.getField();
- this.referenceAtTCA = referenceAtTCA;
- this.referenceCovariance = referenceCovariance;
- this.otherAtTCA = otherAtTCA;
- this.otherCovariance = otherCovariance;
- this.combinedRadius = combinedRadius;
- this.encounterFrame = encounterFrameType.getFrame(otherAtTCA.getPVCoordinates());
- } else {
- throw new OrekitException(OrekitMessages.DIFFERENT_TIME_OF_CLOSEST_APPROACH);
- }
- }
- /**
- * Compute the squared Mahalanobis distance.
- *
- * @param xm other collision object projected xm position onto the collision plane in the rotated encounter frame
- * @param ym other collision object projected ym position onto the collision plane in the rotated encounter frame
- * @param sigmaX square root of the x-axis eigen value of the diagonalized combined covariance matrix projected onto the
- * collision plane
- * @param sigmaY square root of the y-axis eigen value of the diagonalized combined covariance matrix projected onto the
- * collision plane
- * @param <T> type of the field elements
- *
- * @return squared Mahalanobis distance
- */
- public static <T extends CalculusFieldElement<T>> T computeSquaredMahalanobisDistance(final T xm, final T ym,
- final T sigmaX, final T sigmaY) {
- final T[][] positionData = MathArrays.buildArray(xm.getField(), 2, 1);
- positionData[0][0] = xm;
- positionData[1][0] = ym;
- final FieldVector2D<T> position = new FieldVector2D<>(xm, ym);
- final T[][] covarianceMatrixData = MathArrays.buildArray(sigmaX.getField(), 2, 2);
- covarianceMatrixData[0][0] = sigmaX.multiply(sigmaX);
- covarianceMatrixData[1][1] = sigmaY.multiply(sigmaY);
- final FieldMatrix<T> covariance = new BlockFieldMatrix<>(covarianceMatrixData);
- return computeSquaredMahalanobisDistance(position, covariance);
- }
- /**
- * Compute the squared Mahalanobis distance.
- *
- * @param otherPosition other collision object projected position onto the collision plane in the rotated encounter
- * frame
- * @param covarianceMatrix combined covariance matrix projected onto the collision plane and diagonalized
- * @param <T> type of the field elements
- *
- * @return squared Mahalanobis distance
- */
- public static <T extends CalculusFieldElement<T>> T computeSquaredMahalanobisDistance(
- final FieldVector2D<T> otherPosition,
- final FieldMatrix<T> covarianceMatrix) {
- final FieldMatrix<T> covarianceMatrixInverse = new FieldLUDecomposition<>(covarianceMatrix).getSolver().getInverse();
- final FieldMatrix<T> otherPositionOnCollisionPlaneMatrix = new Array2DRowFieldMatrix<>(otherPosition.toArray());
- return otherPositionOnCollisionPlaneMatrix.transposeMultiply(
- covarianceMatrixInverse.multiply(otherPositionOnCollisionPlaneMatrix)).getEntry(0, 0);
- }
- /**
- * Compute the other collision position and velocity relative to the reference collision object. Expressed in the
- * reference collision object inertial frame.
- *
- * @return other collision position and velocity relative to the reference collision object, expressed in the reference
- * collision object inertial frame.
- */
- public FieldPVCoordinates<T> computeOtherRelativeToReferencePVInReferenceInertial() {
- // Extract reference inertial frame
- final Frame referenceInertial = referenceAtTCA.getFrame();
- // Get PVCoordinates in the same frame
- final FieldPVCoordinates<T> referencePV = referenceAtTCA.getPVCoordinates();
- final FieldKinematicTransform<T> kinematicTransform = otherAtTCA.getFrame()
- .getKinematicTransformTo(referenceInertial, referenceAtTCA.getDate());
- final FieldPVCoordinates<T> otherPVInReferenceInertial = kinematicTransform
- .transformOnlyPV(otherAtTCA.getPVCoordinates());
- // Create relative pv expressed in the reference inertial frame
- final FieldVector3D<T> relativePosition = otherPVInReferenceInertial.getPosition()
- .subtract(referencePV.getPosition());
- final FieldVector3D<T> relativeVelocity = otherPVInReferenceInertial.getVelocity()
- .subtract(referencePV.getVelocity());
- return new FieldPVCoordinates<>(relativePosition, relativeVelocity);
- }
- /**
- * Compute the projection matrix from the reference collision object inertial frame to the collision plane.
- * <p>
- * Note that this matrix will only rotate from the reference collision object inertial frame to the encounter frame and
- * project onto the collision plane, this is only a rotation.
- * </p>
- *
- * @return projection matrix from the reference collision object inertial frame to the collision plane
- */
- public FieldMatrix<T> computeReferenceInertialToCollisionPlaneProjectionMatrix() {
- // Create transform from reference inertial frame to encounter local orbital frame
- final FieldStaticTransform<T> referenceInertialToEncounterFrameTransform =
- FieldStaticTransform.compose(tca,
- computeReferenceInertialToReferenceTNWTransform(),
- computeReferenceTNWToEncounterFrameTransform());
- // Create rotation matrix from reference inertial frame to encounter local orbital frame
- final FieldMatrix<T> referenceInertialToEncounterFrameRotationMatrix =
- new Array2DRowFieldMatrix<>(referenceInertialToEncounterFrameTransform.getRotation().getMatrix());
- // Create projection matrix from encounter frame to collision plane
- final FieldMatrix<T> encounterFrameToCollisionPlaneProjectionMatrix =
- encounterFrame.computeProjectionMatrix(tca.getField());
- // Create projection matrix from reference inertial frame to collision plane
- return encounterFrameToCollisionPlaneProjectionMatrix.multiply(referenceInertialToEncounterFrameRotationMatrix);
- }
- /**
- * Compute the combined covariance matrix diagonalized and projected onto the collision plane.
- * <p>
- * Diagonalize projected positional covariance matrix in a specific manner to have
- * <var>σ<sub>xx</sub><sup>2</sup> ≤ σ<sub>yy</sub><sup>2</sup></var>.
- *
- * @return combined covariance matrix diagonalized and projected onto the collision plane
- */
- public FieldMatrix<T> computeProjectedAndDiagonalizedCombinedPositionalCovarianceMatrix() {
- final FieldMatrix<T> covarianceMatrixToDiagonalize = computeProjectedCombinedPositionalCovarianceMatrix();
- final T sigmaXSquared = covarianceMatrixToDiagonalize.getEntry(0, 0);
- final T sigmaYSquared = covarianceMatrixToDiagonalize.getEntry(1, 1);
- final T crossTerm = covarianceMatrixToDiagonalize.getEntry(0, 1);
- final T recurrentTerm = sigmaXSquared.subtract(sigmaYSquared).multiply(0.5).pow(2)
- .add(crossTerm.square()).sqrt();
- final T eigenValueX = sigmaXSquared.add(sigmaYSquared).multiply(0.5).subtract(recurrentTerm);
- final T eigenValueY = sigmaXSquared.add(sigmaYSquared).multiply(0.5).add(recurrentTerm);
- final FieldMatrix<T> projectedAndDiagonalizedCombinedPositionalCovarianceMatrix =
- new BlockFieldMatrix<>(instanceField, 2, 2);
- projectedAndDiagonalizedCombinedPositionalCovarianceMatrix.setEntry(0, 0, eigenValueX);
- projectedAndDiagonalizedCombinedPositionalCovarianceMatrix.setEntry(0, 1, instanceField.getZero());
- projectedAndDiagonalizedCombinedPositionalCovarianceMatrix.setEntry(1, 0, instanceField.getZero());
- projectedAndDiagonalizedCombinedPositionalCovarianceMatrix.setEntry(1, 1, eigenValueY);
- return projectedAndDiagonalizedCombinedPositionalCovarianceMatrix;
- }
- /**
- * Compute the projected combined covariance matrix onto the collision plane.
- *
- * @return projected combined covariance matrix onto the collision plane
- */
- public FieldMatrix<T> computeProjectedCombinedPositionalCovarianceMatrix() {
- // Compute the positional covariance in the encounter local orbital frame
- final FieldMatrix<T> combinedPositionalCovarianceMatrixInEncounterFrame =
- computeCombinedCovarianceInEncounterFrame().getMatrix().getSubMatrix(0, 2, 0, 2);
- // Project it onto the collision plane
- return encounterFrame.projectOntoCollisionPlane(combinedPositionalCovarianceMatrixInEncounterFrame);
- }
- /**
- * Compute the combined covariance expressed in the encounter frame.
- *
- * @return combined covariance expressed in the encounter frame
- */
- public FieldStateCovariance<T> computeCombinedCovarianceInEncounterFrame() {
- return computeCombinedCovarianceInReferenceTNW().changeCovarianceFrame(referenceAtTCA, encounterFrame);
- }
- /**
- * Compute the other collision object {@link FieldVector2D position} projected onto the collision plane.
- *
- * @return other collision object position projected onto the collision plane
- */
- public FieldVector2D<T> computeOtherPositionInCollisionPlane() {
- // Express other in reference inertial
- final FieldVector3D<T> otherInReferenceInertial = otherAtTCA.getPosition(referenceAtTCA.getFrame());
- // Express other in reference TNW local orbital frame
- final FieldVector3D<T> otherPositionInReferenceTNW =
- computeReferenceInertialToReferenceTNWTransform().transformPosition(otherInReferenceInertial);
- // Express other in encounter local orbital frame
- final FieldVector3D<T> otherPositionInEncounterFrame =
- computeReferenceTNWToEncounterFrameTransform().transformPosition(otherPositionInReferenceTNW);
- return encounterFrame.projectOntoCollisionPlane(otherPositionInEncounterFrame);
- }
- /**
- * Compute the other collision object {@link FieldVector2D position} in the rotated collision plane.
- * <p>
- * Uses a default zero threshold of 1e-15.
- * <p>
- * The coordinates are often noted xm and ym in probability of collision related papers.
- * </p>
- * <p>
- * The mentioned rotation concerns the rotation that diagonalize the combined covariance matrix inside the collision
- * plane.
- * </p>
- *
- * @return other collision object position in the rotated collision plane
- */
- public FieldVector2D<T> computeOtherPositionInRotatedCollisionPlane() {
- return computeOtherPositionInRotatedCollisionPlane(DEFAULT_ZERO_THRESHOLD);
- }
- /**
- * Compute the other collision object {@link Vector2D position} in the rotated collision plane.
- * <p>
- * The coordinates are often noted xm and ym in probability of collision related papers.
- * <p>
- * The mentioned rotation concerns the rotation that diagonalize the combined covariance matrix inside the collision
- * plane.
- *
- * @param zeroThreshold threshold below which values are considered equal to zero
- *
- * @return other collision object position in the rotated collision plane
- */
- public FieldVector2D<T> computeOtherPositionInRotatedCollisionPlane(final double zeroThreshold) {
- // Project the other position onto the collision plane
- final FieldMatrix<T> otherPositionInCollisionPlaneMatrix =
- new Array2DRowFieldMatrix<>(computeOtherPositionInCollisionPlane().toArray());
- // Express other in the rotated collision plane
- final FieldMatrix<T> otherPositionRotatedInCollisionPlane =
- computeEncounterPlaneRotationMatrix(zeroThreshold).multiply(otherPositionInCollisionPlaneMatrix);
- return new FieldVector2D<>(otherPositionRotatedInCollisionPlane.getColumn(0));
- }
- /**
- * Compute the Encounter duration (s) evaluated using Coppola's formula described in : "COPPOLA, Vincent, et al.
- * Evaluating the short encounter assumption of the probability of collision formula. 2012."
- * <p>
- * This method is to be used to check the validity of the short-term encounter model. The user is expected to compare the
- * computed duration with the orbital period from both objects and draw its own conclusions.
- * <p>
- * It uses γ = 1e-16 as the resolution of a double is nearly 1e-16 so γ smaller than that are not meaningful to compute.
- *
- * @return encounter duration (s) evaluated using Coppola's formula
- */
- public T computeCoppolaEncounterDuration() {
- // Default value for γ = 1e-16
- final T DEFAULT_ALPHA_C = instanceField.getZero().newInstance(5.864);
- final FieldMatrix<T> combinedPositionalCovarianceMatrix = computeCombinedCovarianceInEncounterFrame()
- .getMatrix().getSubMatrix(0, 2, 0, 2);
- // Extract off-plane cross-term matrix
- final FieldMatrix<T> projectionMatrix = encounterFrame.computeProjectionMatrix(instanceField);
- final FieldMatrix<T> axisNormalToCollisionPlane =
- new Array2DRowFieldMatrix<>(encounterFrame.getAxisNormalToCollisionPlane(instanceField).toArray());
- final FieldMatrix<T> offPlaneCrossTermMatrix =
- projectionMatrix.multiply(combinedPositionalCovarianceMatrix.multiply(axisNormalToCollisionPlane));
- // Covariance sub-matrix of the in-plane terms
- final FieldMatrix<T> probabilityDensity =
- encounterFrame.projectOntoCollisionPlane(combinedPositionalCovarianceMatrix);
- final FieldMatrix<T> probabilityDensityInverse =
- new FieldLUDecomposition<>(probabilityDensity).getSolver().getInverse();
- // Recurrent term in Coppola's paper : bᵀb
- final FieldMatrix<T> b = offPlaneCrossTermMatrix.transposeMultiply(probabilityDensityInverse).transpose();
- final T recurrentTerm = b.multiplyTransposed(b).getEntry(0, 0);
- // Position uncertainty normal to collision plane
- final T sigmaSqNormalToPlan = axisNormalToCollisionPlane.transposeMultiply(
- combinedPositionalCovarianceMatrix.multiply(axisNormalToCollisionPlane)).getEntry(0, 0);
- final T sigmaV = sigmaSqNormalToPlan.subtract(b.multiplyTransposed(offPlaneCrossTermMatrix).getEntry(0, 0))
- .sqrt();
- final T relativeVelocity = computeOtherRelativeToReferencePVInReferenceInertial().getVelocity().getNorm();
- return DEFAULT_ALPHA_C.multiply(sigmaV).multiply(2 * FastMath.sqrt(2)).add(
- combinedRadius.multiply(recurrentTerm.add(1).sqrt().add(recurrentTerm.sqrt()))).divide(relativeVelocity);
- }
- /**
- * Compute the miss distance at time of closest approach.
- *
- * @return miss distance
- */
- public T computeMissDistance() {
- // Get positions expressed in the same frame at time of closest approach
- final FieldVector3D<T> referencePositionAtTCA = referenceAtTCA.getPosition();
- final FieldVector3D<T> otherPositionAtTCA = otherAtTCA.getPosition(referenceAtTCA.getFrame());
- // Compute relative position
- final FieldVector3D<T> relativePosition = otherPositionAtTCA.subtract(referencePositionAtTCA);
- return relativePosition.getNorm();
- }
- /**
- * Compute the Mahalanobis distance computed with the other collision object projected onto the collision plane (commonly
- * called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix is
- * diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
- * <p>
- * Uses a default zero threshold of 1e-15 for the computation of the diagonalizing of the projected covariance matrix.
- *
- * @return Mahalanobis distance between the reference and other collision object
- *
- * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
- */
- public T computeMahalanobisDistance() {
- return computeMahalanobisDistance(DEFAULT_ZERO_THRESHOLD);
- }
- /**
- * Compute the Mahalanobis distance computed with the other collision object projected onto the collision plane (commonly
- * called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix is
- * diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
- *
- * @param zeroThreshold threshold below which values are considered equal to zero
- *
- * @return Mahalanobis distance between the reference and other collision object
- *
- * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
- */
- public T computeMahalanobisDistance(final double zeroThreshold) {
- return computeSquaredMahalanobisDistance(zeroThreshold).sqrt();
- }
- /**
- * Compute the squared Mahalanobis distance computed with the other collision object projected onto the collision plane
- * (commonly called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix
- * is diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
- * <p>
- * Uses a default zero threshold of 1e-15 for the computation of the diagonalizing of the projected covariance matrix.
- *
- * @return squared Mahalanobis distance between the reference and other collision object
- *
- * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
- */
- public T computeSquaredMahalanobisDistance() {
- return computeSquaredMahalanobisDistance(DEFAULT_ZERO_THRESHOLD);
- }
- /**
- * Compute the squared Mahalanobis distance computed with the other collision object projected onto the collision plane
- * (commonly called B-Plane) and expressed in the rotated encounter frame (frame in which the combined covariance matrix
- * is diagonalized, see {@link #computeEncounterPlaneRotationMatrix(double)} for more details).
- *
- * @param zeroThreshold threshold below which values are considered equal to zero
- *
- * @return squared Mahalanobis distance between the reference and other collision object
- *
- * @see <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a>
- */
- public T computeSquaredMahalanobisDistance(final double zeroThreshold) {
- final FieldMatrix<T> otherPositionAfterRotationInCollisionPlane =
- new Array2DRowFieldMatrix<>(computeOtherPositionInRotatedCollisionPlane(zeroThreshold).toArray());
- final FieldMatrix<T> inverseCovarianceMatrix =
- new FieldLUDecomposition<>(
- computeProjectedAndDiagonalizedCombinedPositionalCovarianceMatrix()).getSolver()
- .getInverse();
- return otherPositionAfterRotationInCollisionPlane.transpose().multiply(
- inverseCovarianceMatrix.multiply(
- otherPositionAfterRotationInCollisionPlane))
- .getEntry(0, 0);
- }
- /** Get new encounter instance.
- * @return new encounter instance
- */
- public ShortTermEncounter2DDefinition toEncounter() {
- return new ShortTermEncounter2DDefinition(referenceAtTCA.toOrbit(), referenceCovariance.toStateCovariance(),
- otherAtTCA.toOrbit(), otherCovariance.toStateCovariance(),
- combinedRadius.getReal());
- }
- /**
- * Takes both covariance matrices (expressed in their respective RTN local orbital frame) from reference and other
- * collision object with which this instance was created and sum them in the reference collision object TNW local orbital
- * frame.
- *
- * @return combined covariance matrix expressed in the reference collision object TNW local orbital frame
- */
- public FieldStateCovariance<T> computeCombinedCovarianceInReferenceTNW() {
- // Express reference covariance in reference TNW local orbital frame
- final FieldMatrix<T> referenceCovarianceMatrixInTNW =
- referenceCovariance.changeCovarianceFrame(referenceAtTCA, LOFType.TNW_INERTIAL).getMatrix();
- // Express other covariance in reference inertial frame
- final FieldMatrix<T> otherCovarianceMatrixInReferenceInertial =
- otherCovariance.changeCovarianceFrame(otherAtTCA, referenceAtTCA.getFrame()).getMatrix();
- final FieldStateCovariance<T> otherCovarianceInReferenceInertial = new FieldStateCovariance<>(
- otherCovarianceMatrixInReferenceInertial, tca, referenceAtTCA.getFrame(),
- OrbitType.CARTESIAN, PositionAngleType.MEAN);
- // Express other covariance in reference TNW local orbital frame
- final FieldMatrix<T> otherCovarianceMatrixInReferenceTNW = otherCovarianceInReferenceInertial.changeCovarianceFrame(
- referenceAtTCA, LOFType.TNW_INERTIAL).getMatrix();
- // Return the combined covariance expressed in the reference TNW local orbital frame
- return new FieldStateCovariance<>(referenceCovarianceMatrixInTNW.add(otherCovarianceMatrixInReferenceTNW), tca,
- LOFType.TNW_INERTIAL);
- }
- /**
- * Compute the {@link FieldTransform transform} from the reference collision object inertial frame of reference to its
- * TNW local orbital frame.
- *
- * @return transform from the reference collision object inertial frame of reference to its TNW local orbital frame
- */
- private FieldTransform<T> computeReferenceInertialToReferenceTNWTransform() {
- return LOFType.TNW.transformFromInertial(tca, referenceAtTCA.getPVCoordinates());
- }
- /**
- * Compute the {@link FieldTransform transform} from the reference collision object TNW local orbital frame to the
- * encounter frame.
- *
- * @return transform from the reference collision object TNW local orbital frame to the encounter frame
- */
- private FieldTransform<T> computeReferenceTNWToEncounterFrameTransform() {
- return LOF.transformFromLOFInToLOFOut(LOFType.TNW_INERTIAL, encounterFrame, tca,
- referenceAtTCA.getPVCoordinates());
- }
- /**
- * Compute the rotation matrix that diagonalize the combined positional covariance matrix projected onto the collision
- * plane.
- *
- * @param zeroThreshold threshold below which values are considered equal to zero
- *
- * @return rotation matrix that diagonalize the combined covariance matrix projected onto the collision plane
- */
- private FieldMatrix<T> computeEncounterPlaneRotationMatrix(final double zeroThreshold) {
- final FieldMatrix<T> combinedCovarianceMatrixInEncounterFrame =
- computeCombinedCovarianceInEncounterFrame().getMatrix();
- final FieldMatrix<T> combinedPositionalCovarianceMatrixProjectedOntoBPlane =
- encounterFrame.projectOntoCollisionPlane(
- combinedCovarianceMatrixInEncounterFrame.getSubMatrix(0, 2, 0, 2));
- final T sigmaXSquared = combinedPositionalCovarianceMatrixProjectedOntoBPlane.getEntry(0, 0);
- final T sigmaYSquared = combinedPositionalCovarianceMatrixProjectedOntoBPlane.getEntry(1, 1);
- final T crossTerm = combinedPositionalCovarianceMatrixProjectedOntoBPlane.getEntry(0, 1);
- final T correlation = crossTerm.divide(sigmaXSquared.multiply(sigmaYSquared).sqrt());
- // If the matrix is not initially diagonalized
- final T theta;
- if (FastMath.abs(crossTerm).getReal() > zeroThreshold) {
- final T recurrentTerm = sigmaYSquared.subtract(sigmaXSquared).divide(crossTerm.multiply(2));
- theta = recurrentTerm.subtract(correlation.sign().multiply(recurrentTerm.pow(2).add(1).sqrt())).atan();
- }
- // Else, the matrix is already diagonalized
- else {
- // Rotation in order to have sigmaXSquared < sigmaYSquared
- if (sigmaXSquared.subtract(sigmaYSquared).getReal() > 0) {
- theta = tca.getField().getZero().newInstance(MathUtils.SEMI_PI);
- }
- // Else, there is no need for a rotation
- else {
- theta = tca.getField().getZero();
- }
- }
- final T cosTheta = theta.cos();
- final T sinTheta = theta.sin();
- final Array2DRowFieldMatrix<T> rotationMatrix = new Array2DRowFieldMatrix<>(tca.getField(), 2, 2);
- rotationMatrix.setEntry(0, 0, cosTheta);
- rotationMatrix.setEntry(0, 1, sinTheta);
- rotationMatrix.setEntry(1, 0, sinTheta.negate());
- rotationMatrix.setEntry(1, 1, cosTheta);
- return rotationMatrix;
- }
- /**
- * Get the Time of Closest Approach.
- * <p>
- * Commonly called TCA.
- *
- * @return time of closest approach
- */
- public FieldAbsoluteDate<T> getTca() {
- return tca;
- }
- /** Get reference's orbit at time of closest approach.
- * @return reference's orbit at time of closest approach
- */
- public FieldOrbit<T> getReferenceAtTCA() {
- return referenceAtTCA;
- }
- /** Get other's orbit at time of closest approach.
- * @return other's orbit at time of closest approach
- */
- public FieldOrbit<T> getOtherAtTCA() {
- return otherAtTCA;
- }
- /** Get reference's covariance.
- * @return reference's covariance
- */
- public FieldStateCovariance<T> getReferenceCovariance() {
- return referenceCovariance;
- }
- /** Get other's covariance.
- * @return other's covariance
- */
- public FieldStateCovariance<T> getOtherCovariance() {
- return otherCovariance;
- }
- /** Get combined radius.
- * @return combined radius (m)
- */
- public T getCombinedRadius() {
- return combinedRadius;
- }
- /** Get encounter local orbital frame.
- * @return encounter local orbital frame
- */
- public EncounterLOF getEncounterFrame() {
- return encounterFrame;
- }
- }