IodGibbs.java

  1. /* Copyright 2002-2021 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.estimation.iod;

  18. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  19. import org.hipparchus.util.FastMath;
  20. import org.orekit.errors.OrekitException;
  21. import org.orekit.errors.OrekitMessages;
  22. import org.orekit.estimation.measurements.PV;
  23. import org.orekit.estimation.measurements.Position;
  24. import org.orekit.frames.Frame;
  25. import org.orekit.orbits.KeplerianOrbit;
  26. import org.orekit.time.AbsoluteDate;
  27. import org.orekit.utils.PVCoordinates;

  28. /**
  29.  * Gibbs initial orbit determination.
  30.  * An orbit is determined from three position vectors.
  31.  *
  32.  * Reference:
  33.  *  Vallado, D., Fundamentals of Astrodynamics and Applications
  34.  *
  35.  * @author Joris Olympio
  36.  * @since 8.0
  37.  *
  38.  */
  39. public class IodGibbs {

  40.     /** Threshold for checking coplanr vectors. */
  41.     private static final double COPLANAR_THRESHOLD = FastMath.toRadians(5.);

  42.     /** gravitationnal constant. */
  43.     private final double mu;

  44.     /** Creator.
  45.      *
  46.      * @param mu gravitational constant
  47.      */
  48.     public IodGibbs(final double mu) {
  49.         this.mu = mu;
  50.     }

  51.     /** Give an initial orbit estimation, assuming Keplerian motion.
  52.      * All observations should be from the same location.
  53.      *
  54.      * @param frame measurements frame
  55.      * @param p1 First position measurement
  56.      * @param p2 Second position measurement
  57.      * @param p3 Third position measurement
  58.      * @return an initial orbit estimation
  59.      * @since 11.0
  60.      */
  61.     public KeplerianOrbit estimate(final Frame frame, final Position p1, final Position p2, final Position p3) {
  62.         return estimate(frame,
  63.                         p1.getPosition(), p1.getDate(),
  64.                         p2.getPosition(), p2.getDate(),
  65.                         p3.getPosition(), p3.getDate());
  66.     }

  67.     /** Give an initial orbit estimation, assuming Keplerian motion.
  68.      * All observations should be from the same location.
  69.      *
  70.      * @param frame measure frame
  71.      * @param pv1 PV measure 1 taken in frame
  72.      * @param pv2 PV measure 2 taken in frame
  73.      * @param pv3 PV measure 3 taken in frame
  74.      * @return an initial orbit estimation
  75.      */
  76.     public KeplerianOrbit estimate(final Frame frame, final PV pv1, final PV pv2, final PV pv3) {
  77.         return estimate(frame,
  78.                         pv1.getPosition(), pv1.getDate(),
  79.                         pv2.getPosition(), pv2.getDate(),
  80.                         pv3.getPosition(), pv3.getDate());
  81.     }

  82.     /** Give an initial orbit estimation, assuming Keplerian motion.
  83.      * All observations should be from the same location.
  84.      *
  85.      * @param frame measure frame
  86.      * @param r1 position 1 measured in frame
  87.      * @param date1 date of measure 1
  88.      * @param r2 position 2 measured in frame
  89.      * @param date2 date of measure 2
  90.      * @param r3 position 3 measured in frame
  91.      * @param date3 date of measure 3
  92.      * @return an initial orbit estimation
  93.      */
  94.     public KeplerianOrbit estimate(final Frame frame,
  95.                                    final Vector3D r1, final AbsoluteDate date1,
  96.                                    final Vector3D r2, final AbsoluteDate date2,
  97.                                    final Vector3D r3, final AbsoluteDate date3) {
  98.         // Checks measures are not at the same date
  99.         if (date1.equals(date2) || date1.equals(date3) || date2.equals(date3)) {
  100.             throw new OrekitException(OrekitMessages.NON_DIFFERENT_DATES_FOR_OBSERVATIONS, date1, date2, date3,
  101.                     date2.durationFrom(date1), date3.durationFrom(date1), date3.durationFrom(date2));
  102.         }

  103.         // Checks measures are in the same plane
  104.         final double num = r1.normalize().dotProduct(r2.normalize().crossProduct(r3.normalize()));
  105.         final double alpha = FastMath.PI / 2.0 - FastMath.acos(num);
  106.         if (FastMath.abs(alpha) > COPLANAR_THRESHOLD) {
  107.             throw new OrekitException(OrekitMessages.NON_COPLANAR_POINTS);
  108.         }

  109.         final Vector3D D = r1.crossProduct(r2).add(r2.crossProduct(r3).add(r3.crossProduct(r1)));

  110.         final Vector3D N = (r2.crossProduct(r3)).scalarMultiply(r1.getNorm())
  111.                 .add((r3.crossProduct(r1)).scalarMultiply(r2.getNorm()))
  112.                 .add((r1.crossProduct(r2)).scalarMultiply(r3.getNorm()));

  113.         final Vector3D B = D.crossProduct(r2);

  114.         final Vector3D S = r1.scalarMultiply(r2.getNorm() - r3.getNorm())
  115.                 .add(r2.scalarMultiply(r3.getNorm() - r1.getNorm())
  116.                      .add(r3.scalarMultiply(r1.getNorm() - r2.getNorm())));

  117.         // middle velocity
  118.         final double vm = FastMath.sqrt(mu / (N.getNorm() * D.getNorm()));
  119.         final Vector3D vlEci = B.scalarMultiply(vm / r2.getNorm()).add(S.scalarMultiply(vm));

  120.         // compile a new middle point with position, velocity
  121.         final PVCoordinates pv = new PVCoordinates(r2, vlEci);
  122.         final AbsoluteDate date = date2;

  123.         // compute the equivalent Keplerian orbit
  124.         final KeplerianOrbit orbit = new KeplerianOrbit(pv, frame, date, mu);

  125.         //define the reverse orbit
  126.         final PVCoordinates pv2 = new PVCoordinates(r2, vlEci.scalarMultiply(-1));
  127.         final KeplerianOrbit orbit2 = new KeplerianOrbit(pv2, frame, date, mu);

  128.         //check which orbit is correct
  129.         final Vector3D estP3 = orbit.shiftedBy(date3.durationFrom(date2)).
  130.                 getPVCoordinates().getPosition();
  131.         final double dist = estP3.subtract(r3).getNorm();
  132.         final Vector3D estP3_2 = orbit2.shiftedBy(date3.durationFrom(date2)).
  133.                 getPVCoordinates().getPosition();
  134.         final double dist2 = estP3_2.subtract(r3).getNorm();

  135.         if (dist <= dist2) {
  136.             return orbit;
  137.         } else {
  138.             return orbit2;
  139.         }
  140.     }
  141. }