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;

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

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

  45.     /** Empty constructor. */
  46.     public Alfano2005() {
  47.         super(ShortTermEncounter2DPOCMethodType.ALFANO_2005.name());
  48.     }

  50.     /** {@inheritDoc} */
  51.     public ProbabilityOfCollision compute(final double xm, final double ym, final double sigmaX, final double sigmaY,
  52.                                           final double radius) {
  53.         // Computing development order M
  54.         final int developmentOrderM = computeOrderM(xm, ym, sigmaX, sigmaY, radius);

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

  59.         // Computing initial x0
  60.         final double x0 = 0.015 * xStep - radius;

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

  65.         // 2 : mEven
  66.         double mEvenSum = 0.;
  67.         for (int i = 1; i < developmentOrderM; i++) {
  68.             final double x2i = 2. * i * xStep - radius;
  69.             mEvenSum += getRecurrentPart(x2i, xm, ym, sigmaX, sigmaY, radius);
  70.         }

  72.         final double otherTerm = FastMath.exp(-xm * xm / (2 * sigmaX * sigmaX)) * (
  73.                 Erf.erf((-ym + radius) / (FastMath.sqrt(2) * sigmaY)) -
  74.                         Erf.erf((-ym - radius) / (FastMath.sqrt(2) * sigmaY)));

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

  78.         // 3 : mOdd
  79.         double mOddSum = 0.;
  80.         for (int i = 1; i <= developmentOrderM; i++) {
  81.             final double x2i_1 = (2. * i - 1.) * xStep - radius;
  82.             mOddSum += getRecurrentPart(x2i_1, xm, ym, sigmaX, sigmaY, radius);
  83.         }

  85.         final double mOdd = 4. * mOddSum;

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

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

  92.         return new ProbabilityOfCollision(value, getName(), isAMaximumProbabilityOfCollisionMethod());
  93.     }

  95.     /** {@inheritDoc} */
  96.     @Override
  97.     public <T extends CalculusFieldElement<T>> FieldProbabilityOfCollision<T> compute(final T xm, final T ym,
  98.                                                                                       final T sigmaX, final T sigmaY,
  99.                                                                                       final T radius) {
  100.         final T zero = xm.getField().getZero();

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

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

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

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

  114.         // 2 : mEven
  115.         T mEvenSum = zero;
  116.         for (int i = 1; i < developmentOrderM; i++) {
  117.             final T x2i = xStep.multiply(2. * i).subtract(radius);
  118.             mEvenSum = mEvenSum.add(getRecurrentPart(x2i, xm, ym, sigmaX, sigmaY, radius));
  119.         }

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

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

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

  129.         // 3 : mOdd
  130.         T mOddSum = zero;
  131.         for (int i = 1; i <= developmentOrderM; i++) {
  132.             final T x2i_1 = xStep.multiply(2. * i - 1).subtract(radius);
  133.             mOddSum = mOddSum.add(getRecurrentPart(x2i_1, xm, ym, sigmaX, sigmaY, radius));
  134.         }

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

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

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

  143.         return new FieldProbabilityOfCollision<>(value, getName(), isAMaximumProbabilityOfCollisionMethod());
  144.     }

  146.     /**
  147.      * Get the development order M from inputs.
  148.      *
  149.      * @param xm other collision object projected position onto the collision plane in the rotated encounter frame (m)
  150.      * @param ym other collision object projected position onto the collision plane in the rotated encounter frame (m)
  151.      * @param sigmaX square root of the smallest eigen value of the diagonalized combined covariance matrix projected onto
  152.      * the collision plane
  153.      * @param sigmaY square root of the biggest eigen value of the diagonalized combined covariance matrix projected onto the
  154.      * collision plane
  155.      * @param radius sum of primary and secondary collision object equivalent sphere radii (m)
  156.      *
  157.      * @return development order M
  158.      */
  159.     private int computeOrderM(final double xm, final double ym, final double sigmaX, final double sigmaY,
  160.                               final double radius) {
  161.         final int M = (int) (5 * radius / (FastMath.min(FastMath.min(sigmaX, sigmaY),
  162.                                                         FastMath.min(sigmaX, FastMath.sqrt(xm * xm + ym * ym)))));
  163.         final int LOWER_LIMIT = 10;
  164.         final int UPPER_LIMIT = 100000;

  166.         if (M >= UPPER_LIMIT) {
  167.             return UPPER_LIMIT;
  168.         } else if (M <= LOWER_LIMIT) {
  169.             return LOWER_LIMIT;
  170.         } else {
  171.             return M;
  172.         }
  173.     }

  175.     /**
  176.      * Get the development order M from inputs.
  177.      *
  178.      * @param xm other collision object projected position onto the collision plane in the rotated encounter frame x-axis
  179.      * (m)
  180.      * @param ym other collision object projected position onto the collision plane in the rotated encounter frame y-axis
  181.      * (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 radii (m)
  187.      * @param <T> type of the field elements
  188.      *
  189.      * @return development order M
  190.      */
  191.     private <T extends CalculusFieldElement<T>> int computeOrderM(final T xm, final T ym, final T sigmaX, final T sigmaY,
  192.                                                                   final T radius) {

  194.         final double xmR = xm.getReal();
  195.         final double ymR = ym.getReal();
  196.         final int M = (int) (radius.getReal() * 5. / FastMath.min(FastMath.min(sigmaX.getReal(), sigmaY.getReal()),
  197.                                                                   FastMath.min(sigmaX.getReal(),
  198.                                                                                FastMath.sqrt(xmR * xmR + ymR * ymR))));

  200.         final int lowerLimit = 10;
  201.         final int upperLimit = 10000;

  203.         if (M >= upperLimit) {
  204.             return upperLimit;
  205.         } else if (M <= lowerLimit) {
  206.             return lowerLimit;
  207.         } else {
  208.             return M;
  209.         }
  210.     }

  212.     /**
  213.      * Get the recurrent equation from Alfano's method.
  214.      *
  215.      * @param x step
  216.      * @param xm other collision object projected position onto the collision plane in the rotated encounter frame (m)
  217.      * @param ym other collision object projected position onto the collision plane in the rotated encounter frame (m)
  218.      * @param sigmaX square root of the smallest eigen value of the diagonalized combined covariance matrix projected onto
  219.      * the collision plane
  220.      * @param sigmaY square root of the biggest eigen value of the diagonalized combined covariance matrix projected onto the
  221.      * collision plane
  222.      * @param radius sum of primary and secondary collision object equivalent sphere radius (m)
  223.      *
  224.      * @return recurrent equation from Alfano's method
  225.      */
  226.     private double getRecurrentPart(final double x, final double xm, final double ym, final double sigmaX,
  227.                                     final double sigmaY, final double radius) {
  228.         return (Erf.erf((-ym + FastMath.sqrt(radius * radius - x * x)) / (FastMath.sqrt(2) * sigmaY)) -
  229.                 Erf.erf((-ym - FastMath.sqrt(radius * radius - x * x)) / (FastMath.sqrt(2) * sigmaY))) *
  230.                 (FastMath.exp(-(x - xm) * (x - xm) / (2. * sigmaX * sigmaX)) +
  231.                         FastMath.exp(-(x + xm) * (x + xm) / (2. * sigmaX * sigmaX)));
  232.     }

  234.     /**
  235.      * Get the recurrent equation from Alfano's method.
  236.      *
  237.      * @param x step
  238.      * @param xm other collision object projected position onto the collision plane in the rotated encounter frame (m)
  239.      * @param ym other collision object projected position onto the collision plane in the rotated encounter frame (m)
  240.      * @param sigmaX square root of the smallest eigen value of the diagonalized combined covariance matrix projected onto
  241.      * the collision plane
  242.      * @param sigmaY square root of the biggest eigen value of the diagonalized combined covariance matrix projected onto the
  243.      * collision plane
  244.      * @param radius sum of primary and secondary collision object equivalent sphere radius (m)
  245.      * @param <T> type of the field elements
  246.      *
  247.      * @return recurrent equation from Alfano's method
  248.      */
  249.     private <T extends CalculusFieldElement<T>> T getRecurrentPart(final T x, final T xm, final T ym, final T sigmaX,
  250.                                                                    final T sigmaY, final T radius) {
  251.         final T minusTwoSigmaXSquared          = sigmaX.square().multiply(-2.);
  252.         final T radiusSquaredMinusXSquaredSQRT = radius.square().subtract(x.square()).sqrt();
  253.         final T rootTwoSigmaY                  = sigmaY.multiply(FastMath.sqrt(2));
  254.         final T xMinusXm                       = x.subtract(xm);
  255.         final T xPlusXm                        = x.add(xm);

  257.         return Erf.erf(radiusSquaredMinusXSquaredSQRT.subtract(ym).divide(rootTwoSigmaY)).subtract(
  258.                           Erf.erf(radiusSquaredMinusXSquaredSQRT.add(ym).negate().divide(rootTwoSigmaY)))
  259.                   .multiply(xMinusXm.square().divide(minusTwoSigmaXSquared).exp()
  260.                                     .add(xPlusXm.square().divide(minusTwoSigmaXSquared).exp()));
  261.     }

  263.     /** {@inheritDoc} */
  264.     @Override
  265.     public ShortTermEncounter2DPOCMethodType getType() {
  266.         return ShortTermEncounter2DPOCMethodType.ALFANO_2005;
  267.     }

  269. }
Created with JaCoCo 0.8.12.202403310830