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