IodHerrickGibbs.java

  1. /* Copyright 2022-2025 Bryan Cazabonne
  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.  * Bryan Cazabonne 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.estimation.iod;

  19. import org.hipparchus.CalculusFieldElement;
  20. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  21. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  22. import org.hipparchus.util.FastMath;
  23. import org.orekit.errors.OrekitException;
  24. import org.orekit.errors.OrekitMessages;
  25. import org.orekit.estimation.measurements.PV;
  26. import org.orekit.estimation.measurements.Position;
  27. import org.orekit.frames.Frame;
  28. import org.orekit.orbits.CartesianOrbit;
  29. import org.orekit.orbits.FieldCartesianOrbit;
  30. import org.orekit.orbits.FieldOrbit;
  31. import org.orekit.orbits.Orbit;
  32. import org.orekit.time.AbsoluteDate;
  33. import org.orekit.time.FieldAbsoluteDate;
  34. import org.orekit.utils.FieldPVCoordinates;
  35. import org.orekit.utils.PVCoordinates;

  37. /**
  38.  * HerrickGibbs position-based Initial Orbit Determination (IOD) algorithm.
  39.  * <p>
  40.  * An orbit is determined from three position vectors. Because Gibbs IOD algorithm
  41.  * is limited when the position vectors are to close to one other, Herrick-Gibbs
  42.  * IOD algorithm is a variation made to address this limitation.
  43.  * Because this method is only approximate, it is not robust as the Gibbs method
  44.  * for other cases.
  45.  * </p>
  46.  *
  47.  * @see "Vallado, D., Fundamentals of Astrodynamics and Applications, 4th Edition."
  48.  *
  49.  * @author Bryan Cazabonne
  50.  * @since 13.1
  51.  */
  52. public class IodHerrickGibbs {

  54.     /** Gravitational constant. **/
  55.     private final double mu;

  57.     /** Threshold for checking coplanar vectors (Ref: Equation 7-27). */
  58.     private final double COPLANAR_THRESHOLD = FastMath.toRadians(3.);

  60.     /** Constructor.
  61.      * @param mu gravitational constant
  62.      */
  63.     public IodHerrickGibbs(final double mu) {
  64.         this.mu = mu;
  65.     }

  67.     /** Give an initial orbit estimation, assuming Keplerian motion.
  68.      * <p>
  69.      * All observations should be from the same location.
  70.      * </p>
  71.      * @param frame measurements frame, used as output orbit frame
  72.      * @param p1 First position measurement
  73.      * @param p2 Second position measurement
  74.      * @param p3 Third position measurement
  75.      * @return an initial orbit estimation at the central date
  76.      *         (i.e., date of the second position measurement)
  77.      */
  78.     public Orbit estimate(final Frame frame, final Position p1, final Position p2, final Position p3) {
  79.         return estimate(frame, p1.getPosition(), p1.getDate(),
  80.                         p2.getPosition(), p2.getDate(),
  81.                         p3.getPosition(), p3.getDate());
  82.     }

  84.     /** Give an initial orbit estimation, assuming Keplerian motion.
  85.      * <p>
  86.      * All observations should be from the same location.
  87.      * </p>
  88.      * @param frame measurements frame, used as output orbit frame
  89.      * @param pv1 First PV measurement
  90.      * @param pv2 Second PV measurement
  91.      * @param pv3 Third PV measurement
  92.      * @return an initial orbit estimation at the central date
  93.      *         (i.e., date of the second PV measurement)
  94.      */
  95.     public Orbit estimate(final Frame frame, final PV pv1, final PV pv2, final PV pv3) {
  96.         return estimate(frame, pv1.getPosition(), pv1.getDate(),
  97.                         pv2.getPosition(), pv2.getDate(),
  98.                         pv3.getPosition(), pv3.getDate());
  99.     }

  101.     /** Give an initial orbit estimation, assuming Keplerian motion.
  102.      * <p>
  103.      * All observations should be from the same location.
  104.      * </p>
  105.      * @param frame measurements frame, used as output orbit frame
  106.      * @param r1 position vector 1, expressed in frame
  107.      * @param date1 epoch of position vector 1
  108.      * @param r2 position vector 2, expressed in frame
  109.      * @param date2 epoch of position vector 2
  110.      * @param r3 position vector 3, expressed in frame
  111.      * @param date3 epoch of position vector 3
  112.      * @return an initial orbit estimation at the central date
  113.      *         (i.e., date of the second position measurement)
  114.      */
  115.     public Orbit estimate(final Frame frame,
  116.                           final Vector3D r1, final AbsoluteDate date1,
  117.                           final Vector3D r2, final AbsoluteDate date2,
  118.                           final Vector3D r3, final AbsoluteDate date3) {

  120.         // Verify that measurements are not at the same date
  121.         verifyMeasurementEpochs(date1, date2, date3);

  123.         // Get the difference of time between the position vectors
  124.         final double tau32 = date3.getDate().durationFrom(date2.getDate());
  125.         final double tau31 = date3.getDate().durationFrom(date1.getDate());
  126.         final double tau21 = date2.getDate().durationFrom(date1.getDate());

  128.         // Check that measurements are in the same plane
  129.         final double num = r1.normalize().dotProduct(r2.normalize().crossProduct(r3.normalize()));
  130.         if (FastMath.abs(FastMath.PI / 2.0 - FastMath.acos(num)) > COPLANAR_THRESHOLD) {
  131.             throw new OrekitException(OrekitMessages.NON_COPLANAR_POINTS);
  132.         }

  134.         // Compute velocity vector
  135.         final double muOTwelve = mu / 12.0;
  136.         final double coefficient1 = -tau32 * (1.0 / (tau21 * tau31) + muOTwelve / pow3(r1.getNorm()));
  137.         final double coefficient2 = (tau32 - tau21) * (1.0 / (tau21 * tau32) + muOTwelve / pow3(r2.getNorm()));
  138.         final double coefficient3 = tau21 * (1.0 / (tau32 * tau31) + muOTwelve / pow3(r3.getNorm()));
  139.         final Vector3D v2 = r1.scalarMultiply(coefficient1).add(r2.scalarMultiply(coefficient2)).add(r3.scalarMultiply(coefficient3));

  141.         // Convert to an orbit
  142.         return new CartesianOrbit( new PVCoordinates(r2, v2), frame, date2, mu);
  143.     }

  145.     /** Give an initial orbit estimation, assuming Keplerian motion.
  146.      * <p>
  147.      * All observations should be from the same location.
  148.      * </p>
  149.      * @param frame measurements frame, used as output orbit frame
  150.      * @param r1 position vector 1, expressed in frame
  151.      * @param date1 epoch of position vector 1
  152.      * @param r2 position vector 2, expressed in frame
  153.      * @param date2 epoch of position vector 2
  154.      * @param r3 position vector 3, expressed in frame
  155.      * @param date3 epoch of position vector 3
  156.      * @param <T> type of the elements
  157.      * @return an initial orbit estimation at the central date
  158.      *         (i.e., date of the second position measurement)
  159.      */
  160.     public <T extends CalculusFieldElement<T>> FieldOrbit<T> estimate(final Frame frame,
  161.                                                                       final FieldVector3D<T> r1, final FieldAbsoluteDate<T> date1,
  162.                                                                       final FieldVector3D<T> r2, final FieldAbsoluteDate<T> date2,
  163.                                                                       final FieldVector3D<T> r3, final FieldAbsoluteDate<T> date3) {

  165.         // Verify that measurements are not at the same date
  166.         verifyMeasurementEpochs(date1.toAbsoluteDate(), date2.toAbsoluteDate(), date3.toAbsoluteDate());

  168.         // Get the difference of time between the position vectors
  169.         final T tau32 = date3.getDate().durationFrom(date2.getDate());
  170.         final T tau31 = date3.getDate().durationFrom(date1.getDate());
  171.         final T tau21 = date2.getDate().durationFrom(date1.getDate());

  173.         // Check that measurements are in the same plane
  174.         final T num = r1.normalize().dotProduct(r2.normalize().crossProduct(r3.normalize()));
  175.         if (FastMath.abs(FastMath.PI / 2.0 - FastMath.acos(num.getReal())) > COPLANAR_THRESHOLD) {
  176.             throw new OrekitException(OrekitMessages.NON_COPLANAR_POINTS);
  177.         }

  179.         // Compute velocity vector
  180.         final double muOTwelve = mu / 12.0;
  181.         final T coefficient1 = tau32.negate().multiply(tau21.multiply(tau31).reciprocal().add(pow3(r1.getNorm()).reciprocal().multiply(muOTwelve)));
  182.         final T coefficient2 = tau32.subtract(tau21).multiply(tau21.multiply(tau32).reciprocal().add(pow3(r2.getNorm()).reciprocal().multiply(muOTwelve)));
  183.         final T coefficient3 = tau21.multiply(tau32.multiply(tau31).reciprocal().add(pow3(r3.getNorm()).reciprocal().multiply(muOTwelve)));
  184.         final FieldVector3D<T> v2 = r1.scalarMultiply(coefficient1).add(r2.scalarMultiply(coefficient2)).add(r3.scalarMultiply(coefficient3));

  186.         // Convert to an orbit
  187.         return new FieldCartesianOrbit<>( new FieldPVCoordinates<>(r2, v2), frame, date2, date1.getField().getZero().add(mu));
  188.     }

  190.     /** Compute the cubic value.
  191.      * @param value value
  192.      * @return value^3
  193.      */
  194.     private static double pow3(final double value) {
  195.         return value * value * value;
  196.     }

  198.     /** Compute the cubic value.
  199.      * @param value value
  200.      * @param <T> type of the elements
  201.      * @return value^3
  202.      */
  203.     private static <T extends CalculusFieldElement<T>> T pow3(final T value) {
  204.         return value.multiply(value).multiply(value);
  205.     }

  207.     /** Verifies that measurements are not at the same date.
  208.      * @param date1 epoch of position vector 1
  209.      * @param date2 epoch of position vector 2
  210.      * @param date3 epoch of position vector 3
  211.      */
  212.     private void verifyMeasurementEpochs(final AbsoluteDate date1, final AbsoluteDate date2, final AbsoluteDate date3) {
  213.         if (date1.equals(date2) || date1.equals(date3) || date2.equals(date3)) {
  214.             throw new OrekitException(OrekitMessages.NON_DIFFERENT_DATES_FOR_OBSERVATIONS, date1, date2, date3,
  215.                     date2.durationFrom(date1), date3.durationFrom(date1), date3.durationFrom(date2));
  216.         }
  217.     }
  218. }
Created with JaCoCo 0.8.12.202403310830