ShortTermEncounter2DDefinition.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.geometry.euclidean.threed.Vector3D;
import org.hipparchus.geometry.euclidean.twod.Vector2D;
import org.hipparchus.linear.Array2DRowRealMatrix;
import org.hipparchus.linear.EigenDecompositionSymmetric;
import org.hipparchus.linear.LUDecomposition;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.MathUtils;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitMessages;
import org.orekit.frames.Frame;
import org.orekit.frames.LOF;
import org.orekit.frames.LOFType;
import org.orekit.frames.KinematicTransform;
import org.orekit.frames.StaticTransform;
import org.orekit.frames.Transform;
import org.orekit.frames.encounter.EncounterLOF;
import org.orekit.frames.encounter.EncounterLOFType;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngleType;
import org.orekit.propagation.StateCovariance;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.PVCoordinates;
/**
* 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 ShortTermEncounter2DDefinition encounter = new ShortTermEncounter2DDefinition(primaryOrbitAtTCA, primaryCovariance, primaryRadius, secondaryOrbitAtTCA, secondaryCovariance, 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 ShortTermEncounter2DDefinition encounter = new ShortTermEncounter2DDefinition(secondaryOrbitAtTCA, secondaryCovariance, secondaryRadius, primaryOrbitAtTCA, primaryCovariance, 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
*/
public class ShortTermEncounter2DDefinition {
/** Default threshold below which values are considered equal to zero. */
private static final double DEFAULT_ZERO_THRESHOLD = 1e-15;
/** Default epsilon when checking covariance matrix symmetry. */
private static final double DEFAULT_SYMMETRY_EPSILON = 1e-8;
/**
* Time of closest approach.
* <p>
* Commonly called TCA.
*/
private final AbsoluteDate tca;
/** Reference collision object at time of closest approach. */
private final Orbit referenceAtTCA;
/** Reference collision object covariance matrix in its respective RTN frame. */
private final StateCovariance referenceCovariance;
/** Other collision object at time of closest approach. */
private final Orbit otherAtTCA;
/** Other collision object covariance matrix in its respective RTN frame. */
private final StateCovariance otherCovariance;
/** Combined radius (m). */
private final double 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 ShortTermEncounter2DDefinition(final Orbit referenceAtTCA, final StateCovariance referenceCovariance,
final double referenceRadius, final Orbit otherAtTCA,
final StateCovariance otherCovariance, final double otherRadius) {
this(referenceAtTCA, referenceCovariance, otherAtTCA, otherCovariance, referenceRadius + 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 ShortTermEncounter2DDefinition(final Orbit referenceAtTCA, final StateCovariance referenceCovariance,
final Orbit otherAtTCA, final StateCovariance otherCovariance,
final double 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 ShortTermEncounter2DDefinition(final Orbit referenceAtTCA, final StateCovariance referenceCovariance,
final double referenceRadius, final Orbit otherAtTCA,
final StateCovariance otherCovariance, final double otherRadius,
final EncounterLOFType encounterFrameType, final double tcaTolerance) {
this(referenceAtTCA, referenceCovariance, otherAtTCA, otherCovariance, referenceRadius + 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 ShortTermEncounter2DDefinition(final Orbit referenceAtTCA, final StateCovariance referenceCovariance,
final Orbit otherAtTCA, final StateCovariance otherCovariance,
final double combinedRadius, final EncounterLOFType encounterFrameType,
final double tcaTolerance) {
if (referenceAtTCA.getDate().isCloseTo(otherAtTCA.getDate(), tcaTolerance)) {
this.tca = referenceAtTCA.getDate();
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
*
* @return squared Mahalanobis distance
*/
public static double computeSquaredMahalanobisDistance(final double xm, final double ym,
final double sigmaX, final double sigmaY) {
final Vector2D position = new Vector2D(xm, ym);
final RealMatrix covariance = new Array2DRowRealMatrix(new double[][] {
{ sigmaX * sigmaX, 0 },
{ 0, sigmaY * sigmaY } });
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
*
* @return squared Mahalanobis distance
*/
public static double computeSquaredMahalanobisDistance(final Vector2D otherPosition, final RealMatrix covarianceMatrix) {
final RealMatrix covarianceMatrixInverse = new LUDecomposition(covarianceMatrix).getSolver().getInverse();
final RealMatrix otherPositionOnCollisionPlaneMatrix = new Array2DRowRealMatrix(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 PVCoordinates computeOtherRelativeToReferencePVInReferenceInertial() {
// Extract reference inertial frame
final Frame referenceInertial = referenceAtTCA.getFrame();
// Get PVCoordinates in the same frame
final PVCoordinates referencePV = referenceAtTCA.getPVCoordinates();
final KinematicTransform kinematicTransform = otherAtTCA.getFrame().getKinematicTransformTo(referenceInertial,
otherAtTCA.getDate());
final PVCoordinates otherPVInReferenceInertial = kinematicTransform.transformOnlyPV(otherAtTCA.getPVCoordinates());
// Create relative pv expressed in the reference inertial frame
final Vector3D relativePosition = otherPVInReferenceInertial.getPosition().subtract(referencePV.getPosition());
final Vector3D relativeVelocity = otherPVInReferenceInertial.getVelocity().subtract(referencePV.getVelocity());
return new PVCoordinates(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 RealMatrix computeReferenceInertialToCollisionPlaneProjectionMatrix() {
// Create transform from reference inertial frame to encounter local orbital frame
final StaticTransform referenceInertialToEncounterFrameTransform =
StaticTransform.compose(tca, computeReferenceInertialToReferenceTNWTransform(),
computeReferenceTNWToEncounterFrameTransform());
// Create rotation matrix from reference inertial frame to encounter local orbital frame
final RealMatrix referenceInertialToEncounterFrameRotationMatrix = new Array2DRowRealMatrix(
referenceInertialToEncounterFrameTransform.getRotation().getMatrix());
// Create projection matrix from encounter frame to collision plane
final RealMatrix encounterFrameToCollisionPlaneProjectionMatrix = encounterFrame.computeProjectionMatrix();
// 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 RealMatrix computeProjectedAndDiagonalizedCombinedPositionalCovarianceMatrix() {
final RealMatrix covariance = computeProjectedCombinedPositionalCovarianceMatrix();
final EigenDecompositionSymmetric ed = new EigenDecompositionSymmetric(covariance, DEFAULT_SYMMETRY_EPSILON, false);
return ed.getD();
}
/**
* Compute the projected combined covariance matrix onto the collision plane.
*
* @return projected combined covariance matrix onto the collision plane
*/
public RealMatrix computeProjectedCombinedPositionalCovarianceMatrix() {
// Compute the positional covariance in the encounter local orbital frame
final RealMatrix 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 StateCovariance computeCombinedCovarianceInEncounterFrame() {
return computeCombinedCovarianceInReferenceTNW().changeCovarianceFrame(referenceAtTCA, encounterFrame);
}
/**
* Compute the other collision object {@link Vector2D position} projected onto the collision plane.
*
* @return other collision object position projected onto the collision plane
*/
public Vector2D computeOtherPositionInCollisionPlane() {
// Express other in reference inertial
final Vector3D otherInReferenceInertial = otherAtTCA.getPosition(referenceAtTCA.getFrame());
// Express other in reference TNW local orbital frame
final Vector3D otherPositionInReferenceTNW =
computeReferenceInertialToReferenceTNWTransform().transformPosition(otherInReferenceInertial);
// Express other in encounter local orbital frame
final Vector3D otherPositionInEncounterFrame =
computeReferenceTNWToEncounterFrameTransform().transformPosition(otherPositionInReferenceTNW);
return encounterFrame.projectOntoCollisionPlane(otherPositionInEncounterFrame);
}
/**
* Compute the other collision object {@link Vector2D 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 Vector2D 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 Vector2D computeOtherPositionInRotatedCollisionPlane(final double zeroThreshold) {
// Project the other position onto the collision plane
final RealMatrix otherPositionInCollisionPlaneMatrix =
new Array2DRowRealMatrix(computeOtherPositionInCollisionPlane().toArray());
// Express other in the rotated collision plane
final RealMatrix otherPositionRotatedInCollisionPlane =
computeEncounterPlaneRotationMatrix(zeroThreshold).multiply(otherPositionInCollisionPlaneMatrix);
return new Vector2D(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 double computeCoppolaEncounterDuration() {
// Default value for γ = 1e-16
final double DEFAULT_ALPHA_C = 5.864;
final RealMatrix combinedPositionalCovarianceMatrix = computeCombinedCovarianceInEncounterFrame()
.getMatrix().getSubMatrix(0, 2, 0, 2);
// Extract off-plane cross-term matrix
final RealMatrix projectionMatrix = encounterFrame.computeProjectionMatrix();
final RealMatrix axisNormalToCollisionPlane =
new Array2DRowRealMatrix(encounterFrame.getAxisNormalToCollisionPlane().toArray());
final RealMatrix offPlaneCrossTermMatrix =
projectionMatrix.multiply(combinedPositionalCovarianceMatrix.multiply(axisNormalToCollisionPlane));
// Covariance sub-matrix of the in-plane terms
final RealMatrix probabilityDensity =
encounterFrame.projectOntoCollisionPlane(combinedPositionalCovarianceMatrix);
final RealMatrix probabilityDensityInverse =
new LUDecomposition(probabilityDensity).getSolver().getInverse();
// Recurrent term in Coppola's paper : bᵀb
final RealMatrix b = offPlaneCrossTermMatrix.transposeMultiply(probabilityDensityInverse).transpose();
final double recurrentTerm = b.multiplyTransposed(b).getEntry(0, 0);
// Position uncertainty normal to collision plane
final double sigmaSqNormalToPlan = axisNormalToCollisionPlane.transposeMultiply(
combinedPositionalCovarianceMatrix.multiply(axisNormalToCollisionPlane)).getEntry(0, 0);
final double sigmaV = FastMath.sqrt(
sigmaSqNormalToPlan - b.multiplyTransposed(offPlaneCrossTermMatrix).getEntry(0, 0));
final double relativeVelocity = computeOtherRelativeToReferencePVInReferenceInertial().getVelocity().getNorm();
return (2 * FastMath.sqrt(2) * DEFAULT_ALPHA_C * sigmaV + combinedRadius * (
FastMath.sqrt(1 + recurrentTerm) + FastMath.sqrt(recurrentTerm))) / relativeVelocity;
}
/**
* Compute the miss distance at time of closest approach.
*
* @return miss distance
*/
public double computeMissDistance() {
// Get positions expressed in the same frame at time of closest approach
final Vector3D referencePositionAtTCA = referenceAtTCA.getPosition();
final Vector3D otherPositionAtTCA = otherAtTCA.getPosition(referenceAtTCA.getFrame());
// Compute relative position
final Vector3D 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 double 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 double computeMahalanobisDistance(final double zeroThreshold) {
return FastMath.sqrt(computeSquaredMahalanobisDistance(zeroThreshold));
}
/**
* 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 double 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 double computeSquaredMahalanobisDistance(final double zeroThreshold) {
final RealMatrix otherPositionAfterRotationInCollisionPlane =
new Array2DRowRealMatrix(computeOtherPositionInRotatedCollisionPlane(zeroThreshold).toArray());
final RealMatrix inverseCovarianceMatrix =
new LUDecomposition(computeProjectedAndDiagonalizedCombinedPositionalCovarianceMatrix()).getSolver()
.getInverse();
return otherPositionAfterRotationInCollisionPlane.transpose().multiply(
inverseCovarianceMatrix.multiply(otherPositionAfterRotationInCollisionPlane)).getEntry(0, 0);
}
/**
* 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 StateCovariance computeCombinedCovarianceInReferenceTNW() {
// Express reference covariance in reference TNW local orbital frame
final RealMatrix referenceCovarianceMatrixInTNW =
referenceCovariance.changeCovarianceFrame(referenceAtTCA, LOFType.TNW_INERTIAL).getMatrix();
// Express other covariance in reference inertial frame
final RealMatrix otherCovarianceMatrixInReferenceInertial =
otherCovariance.changeCovarianceFrame(otherAtTCA, referenceAtTCA.getFrame()).getMatrix();
final StateCovariance otherCovarianceInReferenceInertial = new StateCovariance(
otherCovarianceMatrixInReferenceInertial, tca, referenceAtTCA.getFrame(),
OrbitType.CARTESIAN, PositionAngleType.MEAN);
// Express other covariance in reference TNW local orbital frame
final RealMatrix otherCovarianceMatrixInReferenceTNW = otherCovarianceInReferenceInertial.changeCovarianceFrame(
referenceAtTCA, LOFType.TNW_INERTIAL).getMatrix();
// Return the combined covariance expressed in the reference TNW local orbital frame
return new StateCovariance(referenceCovarianceMatrixInTNW.add(otherCovarianceMatrixInReferenceTNW), tca,
LOFType.TNW_INERTIAL);
}
/**
* Compute the {@link Transform 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 Transform computeReferenceInertialToReferenceTNWTransform() {
return LOFType.TNW.transformFromInertial(tca, referenceAtTCA.getPVCoordinates());
}
/**
* Compute the {@link Transform 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 Transform 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 RealMatrix computeEncounterPlaneRotationMatrix(final double zeroThreshold) {
final RealMatrix combinedCovarianceMatrixInEncounterFrame =
computeCombinedCovarianceInEncounterFrame().getMatrix();
final RealMatrix combinedPositionalCovarianceMatrixProjectedOntoBPlane =
encounterFrame.projectOntoCollisionPlane(
combinedCovarianceMatrixInEncounterFrame.getSubMatrix(0, 2, 0, 2));
final double sigmaXSquared = combinedPositionalCovarianceMatrixProjectedOntoBPlane.getEntry(0, 0);
final double sigmaYSquared = combinedPositionalCovarianceMatrixProjectedOntoBPlane.getEntry(1, 1);
final double crossTerm = combinedPositionalCovarianceMatrixProjectedOntoBPlane.getEntry(0, 1);
final double correlation = crossTerm / (FastMath.sqrt(sigmaXSquared * sigmaYSquared));
// If the matrix is not initially diagonalized
final double theta;
if (FastMath.abs(crossTerm) > zeroThreshold) {
final double recurrentTerm = (sigmaYSquared - sigmaXSquared) / (2 * crossTerm);
theta = FastMath.atan(
recurrentTerm - FastMath.signum(correlation) * FastMath.sqrt(1 + FastMath.pow(recurrentTerm, 2)));
}
// Else, the matrix is already diagonalized
else {
// Rotation in order to have sigmaXSquared < sigmaYSquared
if (sigmaXSquared - sigmaYSquared > 0) {
theta = MathUtils.SEMI_PI;
}
// Else, there is no need for a rotation
else {
theta = 0;
}
}
final double[][] collisionPlaneRotationMatrixData = { { FastMath.cos(theta), FastMath.sin(theta) },
{ -FastMath.sin(theta), FastMath.cos(theta) } };
return new Array2DRowRealMatrix(collisionPlaneRotationMatrixData);
}
/**
* Get the Time of Closest Approach.
* <p>
* Commonly called TCA.
*
* @return time of closest approach
*/
public AbsoluteDate getTca() {
return tca;
}
/** Get reference's orbit at time of closest approach.
* @return reference's orbit at time of closest approach
*/
public Orbit getReferenceAtTCA() {
return referenceAtTCA;
}
/** Get other's orbit at time of closest approach.
* @return other's orbit at time of closest approach
*/
public Orbit getOtherAtTCA() {
return otherAtTCA;
}
/** Get reference's covariance.
* @return reference's covariance
*/
public StateCovariance getReferenceCovariance() {
return referenceCovariance;
}
/** Get other's covariance.
* @return other's covariance
*/
public StateCovariance getOtherCovariance() {
return otherCovariance;
}
/** Get combined radius.
* @return combined radius (m)
*/
public double getCombinedRadius() {
return combinedRadius;
}
/** Get encounter local orbital frame.
* @return encounter local orbital frame
*/
public EncounterLOF getEncounterFrame() {
return encounterFrame;
}
}