1   /* Copyright 2002-2022 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  
19  import org.hipparchus.geometry.euclidean.threed.Vector3D;
20  import org.hipparchus.util.FastMath;
21  import org.orekit.errors.OrekitException;
22  import org.orekit.errors.OrekitMessages;
23  import org.orekit.estimation.measurements.PV;
24  import org.orekit.estimation.measurements.Position;
25  import org.orekit.frames.Frame;
26  import org.orekit.orbits.KeplerianOrbit;
27  import org.orekit.time.AbsoluteDate;
28  import org.orekit.utils.PVCoordinates;
29  
30  /**
31   * Gibbs initial orbit determination.
32   * An orbit is determined from three position vectors.
33   *
34   * Reference:
35   *  Vallado, D., Fundamentals of Astrodynamics and Applications
36   *
37   * @author Joris Olympio
38   * @since 8.0
39   *
40   */
41  public class IodGibbs {
42  
43      /** Threshold for checking coplanr vectors. */
44      private static final double COPLANAR_THRESHOLD = FastMath.toRadians(5.);
45  
46      /** gravitationnal constant. */
47      private final double mu;
48  
49      /** Creator.
50       *
51       * @param mu gravitational constant
52       */
53      public IodGibbs(final double mu) {
54          this.mu = mu;
55      }
56  
57      /** Give an initial orbit estimation, assuming Keplerian motion.
58       * All observations should be from the same location.
59       *
60       * @param frame measurements frame
61       * @param p1 First position measurement
62       * @param p2 Second position measurement
63       * @param p3 Third position measurement
64       * @return an initial orbit estimation at the central date
65       *         (i.e., date of the second position measurement)
66       * @since 11.0
67       */
68      public KeplerianOrbit estimate(final Frame frame, final Position p1, final Position p2, final Position p3) {
69          return estimate(frame,
70                          p1.getPosition(), p1.getDate(),
71                          p2.getPosition(), p2.getDate(),
72                          p3.getPosition(), p3.getDate());
73      }
74  
75      /** Give an initial orbit estimation, assuming Keplerian motion.
76       * All observations should be from the same location.
77       *
78       * @param frame measure frame
79       * @param pv1 PV measure 1 taken in frame
80       * @param pv2 PV measure 2 taken in frame
81       * @param pv3 PV measure 3 taken in frame
82       * @return an initial orbit estimation at the central date
83       *         (i.e., date of the second PV measurement)
84       */
85      public KeplerianOrbit estimate(final Frame frame, final PV pv1, final PV pv2, final PV pv3) {
86          return estimate(frame,
87                          pv1.getPosition(), pv1.getDate(),
88                          pv2.getPosition(), pv2.getDate(),
89                          pv3.getPosition(), pv3.getDate());
90      }
91  
92      /** Give an initial orbit estimation, assuming Keplerian motion.
93       * All observations should be from the same location.
94       *
95       * @param frame measure frame
96       * @param r1 position 1 measured in frame
97       * @param date1 date of measure 1
98       * @param r2 position 2 measured in frame
99       * @param date2 date of measure 2
100      * @param r3 position 3 measured in frame
101      * @param date3 date of measure 3
102      * @return an initial orbit estimation at the central date
103      *         (i.e., date of the second position measurement)
104      */
105     public KeplerianOrbit estimate(final Frame frame,
106                                    final Vector3D r1, final AbsoluteDate date1,
107                                    final Vector3D r2, final AbsoluteDate date2,
108                                    final Vector3D r3, final AbsoluteDate date3) {
109         // Checks measures are not at the same date
110         if (date1.equals(date2) || date1.equals(date3) || date2.equals(date3)) {
111             throw new OrekitException(OrekitMessages.NON_DIFFERENT_DATES_FOR_OBSERVATIONS, date1, date2, date3,
112                     date2.durationFrom(date1), date3.durationFrom(date1), date3.durationFrom(date2));
113         }
114 
115         // Checks measures are in the same plane
116         final double num = r1.normalize().dotProduct(r2.normalize().crossProduct(r3.normalize()));
117         final double alpha = FastMath.PI / 2.0 - FastMath.acos(num);
118         if (FastMath.abs(alpha) > COPLANAR_THRESHOLD) {
119             throw new OrekitException(OrekitMessages.NON_COPLANAR_POINTS);
120         }
121 
122         final Vector3D D = r1.crossProduct(r2).add(r2.crossProduct(r3).add(r3.crossProduct(r1)));
123 
124         final Vector3D N = (r2.crossProduct(r3)).scalarMultiply(r1.getNorm())
125                 .add((r3.crossProduct(r1)).scalarMultiply(r2.getNorm()))
126                 .add((r1.crossProduct(r2)).scalarMultiply(r3.getNorm()));
127 
128         final Vector3D B = D.crossProduct(r2);
129 
130         final Vector3D S = r1.scalarMultiply(r2.getNorm() - r3.getNorm())
131                 .add(r2.scalarMultiply(r3.getNorm() - r1.getNorm())
132                      .add(r3.scalarMultiply(r1.getNorm() - r2.getNorm())));
133 
134         // middle velocity
135         final double vm = FastMath.sqrt(mu / (N.getNorm() * D.getNorm()));
136         final Vector3D vlEci = B.scalarMultiply(vm / r2.getNorm()).add(S.scalarMultiply(vm));
137 
138         // compile a new middle point with position, velocity
139         final PVCoordinates pv = new PVCoordinates(r2, vlEci);
140         final AbsoluteDate date = date2;
141 
142         // compute the equivalent Keplerian orbit
143         final KeplerianOrbit orbit = new KeplerianOrbit(pv, frame, date, mu);
144 
145         //define the reverse orbit
146         final PVCoordinates pv2 = new PVCoordinates(r2, vlEci.scalarMultiply(-1));
147         final KeplerianOrbit orbit2 = new KeplerianOrbit(pv2, frame, date, mu);
148 
149         //check which orbit is correct
150         final Vector3D estP3 = orbit.shiftedBy(date3.durationFrom(date2)).
151                 getPVCoordinates().getPosition();
152         final double dist = estP3.subtract(r3).getNorm();
153         final Vector3D estP3_2 = orbit2.shiftedBy(date3.durationFrom(date2)).
154                 getPVCoordinates().getPosition();
155         final double dist2 = estP3_2.subtract(r3).getNorm();
156 
157         if (dist <= dist2) {
158             return orbit;
159         } else {
160             return orbit2;
161         }
162     }
163 }