Alfano2005.java

  1. /* Copyright 2002-2024 CS GROUP
  2.  * Licensed to CS GROUP (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.ssa.collision.shorttermencounter.probability.twod;

  18. import org.hipparchus.CalculusFieldElement;
  19. import org.hipparchus.special.Erf;
  20. import org.hipparchus.util.FastMath;
  21. import org.orekit.ssa.metrics.FieldProbabilityOfCollision;
  22. import org.orekit.ssa.metrics.ProbabilityOfCollision;

  23. /**
  24.  * Compute the probability of collision using the method described in :"S. Alfano. A numerical implementation of spherical
  25.  * objet collision probability. Journal of Astronautical Sciences, 53(1), January-March 2005."
  26.  * <p>
  27.  * It assumes :
  28.  *     <ul>
  29.  *         <li>Short encounter leading to a linear relative motion.</li>
  30.  *         <li>Spherical collision object.</li>
  31.  *         <li>Uncorrelated positional covariance.</li>
  32.  *         <li>Gaussian distribution of the position uncertainties.</li>
  33.  *         <li>Deterministic velocity i.e. no velocity uncertainties.</li>
  34.  *     </ul>
  35.  * <p>
  36.  * Also, it has been implemented using a simpson integration scheme as explained in his paper.
  37.  *
  38.  * @author Vincent Cucchietti
  39.  * @since 12.0
  40.  */
  41. public class Alfano2005 extends AbstractShortTermEncounter2DPOCMethod {

  42.     /** Empty constructor. */
  43.     public Alfano2005() {
  44.         super(ShortTermEncounter2DPOCMethodType.ALFANO_2005.name());
  45.     }

  46.     /** {@inheritDoc} */
  47.     public ProbabilityOfCollision compute(final double xm, final double ym, final double sigmaX, final double sigmaY,
  48.                                           final double radius) {
  49.         // Computing development order M
  50.         final int developmentOrderM = computeOrderM(xm, ym, sigmaX, sigmaY, radius);

  51.         // Computing x step
  52.         final double xStep = radius / (2 * developmentOrderM);

  53.         // Computing initial x0
  54.         final double x0 = 0.015 * xStep - radius;

  55.         // 1 : m0
  56.         final double m0 = 2. * getRecurrentPart(x0, xm, ym, sigmaX, sigmaY, radius);

  57.         // 2 : mEven
  58.         double mEvenSum = 0.;
  59.         for (int i = 1; i < developmentOrderM; i++) {
  60.             final double x2i = 2. * i * xStep - radius;
  61.             mEvenSum += getRecurrentPart(x2i, xm, ym, sigmaX, sigmaY, radius);
  62.         }

  63.         final double otherTerm = FastMath.exp(-xm * xm / (2 * sigmaX * sigmaX)) * (
  64.                 Erf.erf((-ym + radius) / (FastMath.sqrt(2) * sigmaY)) -
  65.                         Erf.erf((-ym - radius) / (FastMath.sqrt(2) * sigmaY)));

  66.         final double mEven = 2. * (mEvenSum + otherTerm);

  67.         // 3 : mOdd
  68.         double mOddSum = 0.;
  69.         for (int i = 1; i <= developmentOrderM; i++) {
  70.             final double x2i_1 = (2. * i - 1.) * xStep - radius;
  71.             mOddSum += getRecurrentPart(x2i_1, xm, ym, sigmaX, sigmaY, radius);
  72.         }

  73.         final double mOdd = 4. * mOddSum;

  74.         // Output
  75.         final double factor = xStep / (3. * sigmaX * FastMath.sqrt(8. * FastMath.PI));

  76.         final double value = factor * (m0 + mEven + mOdd);

  77.         return new ProbabilityOfCollision(value, getName(), isAMaximumProbabilityOfCollisionMethod());
  78.     }

  79.     /** {@inheritDoc} */
  80.     @Override
  81.     public <T extends CalculusFieldElement<T>> FieldProbabilityOfCollision<T> compute(final T xm, final T ym,
  82.                                                                                       final T sigmaX, final T sigmaY,
  83.                                                                                       final T radius) {
  84.         final T zero = xm.getField().getZero();

  85.         // Computing development order M
  86.         final int developmentOrderM = computeOrderM(xm, ym, sigmaX, sigmaY, radius);

  87.         // Computing x step
  88.         final T xStep = radius.multiply(0.5 / developmentOrderM);

  89.         // Computing initial x0
  90.         final T x0 = xStep.multiply(0.015).subtract(radius);

  91.         // 1 : m0
  92.         final T m0 = getRecurrentPart(x0, xm, ym, sigmaX, sigmaY, radius).multiply(2.);

  93.         // 2 : mEven
  94.         T mEvenSum = zero;
  95.         for (int i = 1; i < developmentOrderM; i++) {
  96.             final T x2i = xStep.multiply(2. * i).subtract(radius);
  97.             mEvenSum = mEvenSum.add(getRecurrentPart(x2i, xm, ym, sigmaX, sigmaY, radius));
  98.         }

  99.         final T rootTwoSigmaY = sigmaY.multiply(FastMath.sqrt(2));

  100.         final T otherTerm = xm.square().divide(sigmaX.multiply(sigmaX).multiply(-2.)).exp()
  101.                               .multiply(Erf.erf(radius.subtract(ym).divide(rootTwoSigmaY))
  102.                                            .subtract(Erf.erf(radius.add(ym).negate().divide(rootTwoSigmaY))));

  103.         final T mEven = mEvenSum.add(otherTerm).multiply(2.);

  104.         // 3 : mOdd
  105.         T mOddSum = zero;
  106.         for (int i = 1; i <= developmentOrderM; i++) {
  107.             final T x2i_1 = xStep.multiply(2. * i - 1).subtract(radius);
  108.             mOddSum = mOddSum.add(getRecurrentPart(x2i_1, xm, ym, sigmaX, sigmaY, radius));
  109.         }

  110.         final T mOdd = mOddSum.multiply(4.);

  111.         // Output
  112.         final T factor = xStep.divide(sigmaX.multiply(3 * FastMath.sqrt(8. * FastMath.PI)));

  113.         final T value = factor.multiply(m0.add(mEven).add(mOdd));

  114.         return new FieldProbabilityOfCollision<>(value, getName(), isAMaximumProbabilityOfCollisionMethod());
  115.     }

  116.     /**
  117.      * Get the development order M from inputs.
  118.      *
  119.      * @param xm other collision object projected position onto the collision plane in the rotated encounter frame (m)
  120.      * @param ym other collision object projected position onto the collision plane in the rotated encounter frame (m)
  121.      * @param sigmaX square root of the smallest eigen value of the diagonalized combined covariance matrix projected onto
  122.      * the collision plane
  123.      * @param sigmaY square root of the biggest eigen value of the diagonalized combined covariance matrix projected onto the
  124.      * collision plane
  125.      * @param radius sum of primary and secondary collision object equivalent sphere radii (m)
  126.      *
  127.      * @return development order M
  128.      */
  129.     private int computeOrderM(final double xm, final double ym, final double sigmaX, final double sigmaY,
  130.                               final double radius) {
  131.         final int M = (int) (5 * radius / (FastMath.min(FastMath.min(sigmaX, sigmaY),
  132.                                                         FastMath.min(sigmaX, FastMath.sqrt(xm * xm + ym * ym)))));
  133.         final int LOWER_LIMIT = 10;
  134.         final int UPPER_LIMIT = 100000;

  135.         if (M >= UPPER_LIMIT) {
  136.             return UPPER_LIMIT;
  137.         } else if (M <= LOWER_LIMIT) {
  138.             return LOWER_LIMIT;
  139.         } else {
  140.             return M;
  141.         }
  142.     }

  143.     /**
  144.      * Get the development order M from inputs.
  145.      *
  146.      * @param xm other collision object projected position onto the collision plane in the rotated encounter frame x-axis
  147.      * (m)
  148.      * @param ym other collision object projected position onto the collision plane in the rotated encounter frame y-axis
  149.      * (m)
  150.      * @param sigmaX square root of the smallest eigen value of the diagonalized combined covariance matrix projected onto
  151.      * the collision plane
  152.      * @param sigmaY square root of the biggest eigen value of the diagonalized combined covariance matrix projected onto the
  153.      * collision plane
  154.      * @param radius sum of primary and secondary collision object equivalent sphere radii (m)
  155.      * @param <T> type of the field elements
  156.      *
  157.      * @return development order M
  158.      */
  159.     private <T extends CalculusFieldElement<T>> int computeOrderM(final T xm, final T ym, final T sigmaX, final T sigmaY,
  160.                                                                   final T radius) {

  161.         final double xmR = xm.getReal();
  162.         final double ymR = ym.getReal();
  163.         final int M = (int) (radius.getReal() * 5. / FastMath.min(FastMath.min(sigmaX.getReal(), sigmaY.getReal()),
  164.                                                                   FastMath.min(sigmaX.getReal(),
  165.                                                                                FastMath.sqrt(xmR * xmR + ymR * ymR))));

  166.         final int lowerLimit = 10;
  167.         final int upperLimit = 10000;

  168.         if (M >= upperLimit) {
  169.             return upperLimit;
  170.         } else if (M <= lowerLimit) {
  171.             return lowerLimit;
  172.         } else {
  173.             return M;
  174.         }
  175.     }

  176.     /**
  177.      * Get the recurrent equation from Alfano's method.
  178.      *
  179.      * @param x step
  180.      * @param xm other collision object projected position onto the collision plane in the rotated encounter frame (m)
  181.      * @param ym other collision object projected position onto the collision plane in the rotated encounter frame (m)
  182.      * @param sigmaX square root of the smallest eigen value of the diagonalized combined covariance matrix projected onto
  183.      * the collision plane
  184.      * @param sigmaY square root of the biggest eigen value of the diagonalized combined covariance matrix projected onto the
  185.      * collision plane
  186.      * @param radius sum of primary and secondary collision object equivalent sphere radius (m)
  187.      *
  188.      * @return recurrent equation from Alfano's method
  189.      */
  190.     private double getRecurrentPart(final double x, final double xm, final double ym, final double sigmaX,
  191.                                     final double sigmaY, final double radius) {
  192.         return (Erf.erf((-ym + FastMath.sqrt(radius * radius - x * x)) / (FastMath.sqrt(2) * sigmaY)) -
  193.                 Erf.erf((-ym - FastMath.sqrt(radius * radius - x * x)) / (FastMath.sqrt(2) * sigmaY))) *
  194.                 (FastMath.exp(-(x - xm) * (x - xm) / (2. * sigmaX * sigmaX)) +
  195.                         FastMath.exp(-(x + xm) * (x + xm) / (2. * sigmaX * sigmaX)));
  196.     }

  197.     /**
  198.      * Get the recurrent equation from Alfano's method.
  199.      *
  200.      * @param x step
  201.      * @param xm other collision object projected position onto the collision plane in the rotated encounter frame (m)
  202.      * @param ym other collision object projected position onto the collision plane in the rotated encounter frame (m)
  203.      * @param sigmaX square root of the smallest eigen value of the diagonalized combined covariance matrix projected onto
  204.      * the collision plane
  205.      * @param sigmaY square root of the biggest eigen value of the diagonalized combined covariance matrix projected onto the
  206.      * collision plane
  207.      * @param radius sum of primary and secondary collision object equivalent sphere radius (m)
  208.      * @param <T> type of the field elements
  209.      *
  210.      * @return recurrent equation from Alfano's method
  211.      */
  212.     private <T extends CalculusFieldElement<T>> T getRecurrentPart(final T x, final T xm, final T ym, final T sigmaX,
  213.                                                                    final T sigmaY, final T radius) {
  214.         final T minusTwoSigmaXSquared          = sigmaX.square().multiply(-2.);
  215.         final T radiusSquaredMinusXSquaredSQRT = radius.square().subtract(x.square()).sqrt();
  216.         final T rootTwoSigmaY                  = sigmaY.multiply(FastMath.sqrt(2));
  217.         final T xMinusXm                       = x.subtract(xm);
  218.         final T xPlusXm                        = x.add(xm);

  219.         return Erf.erf(radiusSquaredMinusXSquaredSQRT.subtract(ym).divide(rootTwoSigmaY)).subtract(
  220.                           Erf.erf(radiusSquaredMinusXSquaredSQRT.add(ym).negate().divide(rootTwoSigmaY)))
  221.                   .multiply(xMinusXm.square().divide(minusTwoSigmaXSquared).exp()
  222.                                     .add(xPlusXm.square().divide(minusTwoSigmaXSquared).exp()));
  223.     }

  224.     /** {@inheritDoc} */
  225.     @Override
  226.     public ShortTermEncounter2DPOCMethodType getType() {
  227.         return ShortTermEncounter2DPOCMethodType.ALFANO_2005;
  228.     }

  229. }