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.CartesianOrbit;
27 import org.orekit.orbits.Orbit;
28 import org.orekit.time.AbsoluteDate;
29 import org.orekit.utils.PVCoordinates;
30
31 /**
32 * Lambert position-based Initial Orbit Determination (IOD) algorithm, assuming Keplerian motion.
33 * <p>
34 * An orbit is determined from two position vectors.
35 *
36 * References:
37 * Battin, R.H., An Introduction to the Mathematics and Methods of Astrodynamics, AIAA Education, 1999.
38 * Lancaster, E.R. and Blanchard, R.C., A Unified Form of Lambert’s Theorem, Goddard Space Flight Center, 1968.
39 * </p>
40 * @author Joris Olympio
41 * @since 8.0
42 */
43 public class IodLambert {
44
45 /** gravitational constant. */
46 private final double mu;
47
48 /** Creator.
49 *
50 * @param mu gravitational constant
51 */
52 public IodLambert(final double mu) {
53 this.mu = mu;
54 }
55
56 /** Estimate an initial orbit from two position measurements.
57 * <p>
58 * The logic for setting {@code posigrade} and {@code nRev} is that the
59 * sweep angle Δυ travelled by the object between {@code t1} and {@code t2} is
60 * 2π {@code nRev +1} - α if {@code posigrade} is false and 2π {@code nRev} + α
61 * if {@code posigrade} is true, where α is the separation angle between
62 * {@code p1} and {@code p2}, which is always computed between 0 and π
63 * (because in 3D without a normal reference, vector angles cannot go past π).
64 * </p>
65 * <p>
66 * This implies that {@code posigrade} should be set to true if {@code p2} is
67 * located in the half orbit starting at {@code p1} and it should be set to
68 * false if {@code p2} is located in the half orbit ending at {@code p1},
69 * regardless of the number of periods between {@code t1} and {@code t2},
70 * and {@code nRev} should be set accordingly.
71 * </p>
72 * <p>
73 * As an example, if {@code t2} is less than half a period after {@code t1},
74 * then {@code posigrade} should be {@code true} and {@code nRev} should be 0.
75 * If {@code t2} is more than half a period after {@code t1} but less than
76 * one period after {@code t1}, {@code posigrade} should be {@code false} and
77 * {@code nRev} should be 0.
78 * </p>
79 * @param frame measurements frame
80 * @param posigrade flag indicating the direction of motion
81 * @param nRev number of revolutions
82 * @param p1 first position measurement
83 * @param p2 second position measurement
84 * @return an initial Keplerian orbit estimation at the first observation date t1
85 * @since 11.0
86 */
87 public Orbit estimate(final Frame frame, final boolean posigrade,
88 final int nRev, final Position p1, final Position p2) {
89 return estimate(frame, posigrade, nRev,
90 p1.getPosition(), p1.getDate(), p2.getPosition(), p2.getDate());
91 }
92
93 /** Estimate an initial orbit from two PV measurements.
94 * <p>
95 * The logic for setting {@code posigrade} and {@code nRev} is that the
96 * sweep angle Δυ travelled by the object between {@code t1} and {@code t2} is
97 * 2π {@code nRev +1} - α if {@code posigrade} is false and 2π {@code nRev} + α
98 * if {@code posigrade} is true, where α is the separation angle between
99 * {@code p1} and {@code p2}, which is always computed between 0 and π
100 * (because in 3D without a normal reference, vector angles cannot go past π).
101 * </p>
102 * <p>
103 * This implies that {@code posigrade} should be set to true if {@code p2} is
104 * located in the half orbit starting at {@code p1} and it should be set to
105 * false if {@code p2} is located in the half orbit ending at {@code p1},
106 * regardless of the number of periods between {@code t1} and {@code t2},
107 * and {@code nRev} should be set accordingly.
108 * </p>
109 * <p>
110 * As an example, if {@code t2} is less than half a period after {@code t1},
111 * then {@code posigrade} should be {@code true} and {@code nRev} should be 0.
112 * If {@code t2} is more than half a period after {@code t1} but less than
113 * one period after {@code t1}, {@code posigrade} should be {@code false} and
114 * {@code nRev} should be 0.
115 * </p>
116 * @param frame measurements frame
117 * @param posigrade flag indicating the direction of motion
118 * @param nRev number of revolutions
119 * @param pv1 first PV measurement
120 * @param pv2 second PV measurement
121 * @return an initial Keplerian orbit estimation at the first observation date t1
122 * @since 12.0
123 */
124 public Orbit estimate(final Frame frame, final boolean posigrade,
125 final int nRev, final PV pv1, final PV pv2) {
126 return estimate(frame, posigrade, nRev,
127 pv1.getPosition(), pv1.getDate(), pv2.getPosition(), pv2.getDate());
128 }
129
130 /** Estimate a Keplerian orbit given two position vectors and a duration.
131 * <p>
132 * The logic for setting {@code posigrade} and {@code nRev} is that the
133 * sweep angle Δυ travelled by the object between {@code t1} and {@code t2} is
134 * 2π {@code nRev +1} - α if {@code posigrade} is false and 2π {@code nRev} + α
135 * if {@code posigrade} is true, where α is the separation angle between
136 * {@code p1} and {@code p2}, which is always computed between 0 and π
137 * (because in 3D without a normal reference, vector angles cannot go past π).
138 * </p>
139 * <p>
140 * This implies that {@code posigrade} should be set to true if {@code p2} is
141 * located in the half orbit starting at {@code p1} and it should be set to
142 * false if {@code p2} is located in the half orbit ending at {@code p1},
143 * regardless of the number of periods between {@code t1} and {@code t2},
144 * and {@code nRev} should be set accordingly.
145 * </p>
146 * <p>
147 * As an example, if {@code t2} is less than half a period after {@code t1},
148 * then {@code posigrade} should be {@code true} and {@code nRev} should be 0.
149 * If {@code t2} is more than half a period after {@code t1} but less than
150 * one period after {@code t1}, {@code posigrade} should be {@code false} and
151 * {@code nRev} should be 0.
152 * </p>
153 * @param frame frame
154 * @param posigrade flag indicating the direction of motion
155 * @param nRev number of revolutions
156 * @param p1 position vector 1
157 * @param t1 date of observation 1
158 * @param p2 position vector 2
159 * @param t2 date of observation 2
160 * @return an initial Keplerian orbit estimate at the first observation date t1
161 */
162 public Orbit estimate(final Frame frame, final boolean posigrade,
163 final int nRev,
164 final Vector3D p1, final AbsoluteDate t1,
165 final Vector3D p2, final AbsoluteDate t2) {
166
167 final double r1 = p1.getNorm();
168 final double r2 = p2.getNorm();
169 final double tau = t2.durationFrom(t1); // in seconds
170
171 // Exception if t2 < t1
172 if (tau < 0.0) {
173 throw new OrekitException(OrekitMessages.NON_CHRONOLOGICAL_DATES_FOR_OBSERVATIONS, t1, t2, -tau);
174 }
175
176 // normalizing constants
177 final double R = FastMath.max(r1, r2); // in m
178 final double V = FastMath.sqrt(mu / R); // in m/s
179 final double T = R / V; // in seconds
180
181 // sweep angle
182 double dth = Vector3D.angle(p1, p2);
183 // compute the number of revolutions
184 if (!posigrade) {
185 dth = 2 * FastMath.PI - dth;
186 }
187
188 // velocity vectors in the orbital plane, in the R-T frame
189 final double[] Vdep = new double[2];
190
191 // call Lambert's problem solver
192 final boolean exitflag = solveLambertPb(r1 / R, r2 / R, dth, tau / T, nRev, Vdep);
193
194 if (exitflag) {
195 // basis vectors
196 // normal to the orbital arc plane
197 final Vector3D Pn = p1.crossProduct(p2);
198 // perpendicular to the radius vector, in the orbital arc plane
199 final Vector3D Pt = Pn.crossProduct(p1);
200
201 // tangential velocity norm
202 double RT = Pt.getNorm();
203 if (!posigrade) {
204 RT = -RT;
205 }
206
207 // velocity vector at P1
208 final Vector3D Vel1 = new Vector3D(V * Vdep[0] / r1, p1,
209 V * Vdep[1] / RT, Pt);
210
211 // compute the equivalent Cartesian orbit
212 return new CartesianOrbit(new PVCoordinates(p1, Vel1), frame, t1, mu);
213 }
214
215 return null;
216 }
217
218 /** Lambert's solver.
219 * Assume mu=1.
220 *
221 * @param r1 radius 1
222 * @param r2 radius 2
223 * @param dth sweep angle
224 * @param tau time of flight
225 * @param mRev number of revs
226 * @param V1 velocity at departure in (T, N) basis
227 * @return something
228 */
229 boolean solveLambertPb(final double r1, final double r2, final double dth, final double tau,
230 final int mRev, final double[] V1) {
231 // decide whether to use the left or right branch (for multi-revolution
232 // problems), and the long- or short way.
233 final boolean leftbranch = dth < FastMath.PI;
234 int longway = 1;
235 if (dth > FastMath.PI) {
236 longway = -1;
237 }
238
239 final int m = FastMath.abs(mRev);
240 final double rtof = FastMath.abs(tau);
241
242 // non-dimensional chord ||r2-r1||
243 final double chord = FastMath.sqrt(r1 * r1 + r2 * r2 - 2 * r1 * r2 * FastMath.cos(dth));
244
245 // non-dimensional semi-perimeter of the triangle
246 final double speri = 0.5 * (r1 + r2 + chord);
247
248 // minimum energy ellipse semi-major axis
249 final double minSma = speri / 2.;
250
251 // lambda parameter (Eq 7.6)
252 final double lambda = longway * FastMath.sqrt(1 - chord / speri);
253
254 // reference tof value for the Newton solver
255 final double logt = FastMath.log(rtof);
256
257 // initialisation of the iterative root finding process (secant method)
258 // initial values
259 // -1 < x < 1 => Elliptical orbits
260 // x = 1 Parabolic orbit
261 // x > 1 Hyperbolic orbits
262 final double in1;
263 final double in2;
264 double x1;
265 double x2;
266 if (m == 0) {
267 // one revolution, one solution. Define the left and right asymptotes.
268 in1 = -0.6523333;
269 in2 = 0.6523333;
270 x1 = FastMath.log(1 + in1);
271 x2 = FastMath.log(1 + in2);
272 } else {
273 // select initial values, depending on the branch
274 if (!leftbranch) {
275 in1 = -0.523334;
276 in2 = -0.223334;
277 } else {
278 in1 = 0.723334;
279 in2 = 0.523334;
280 }
281 x1 = FastMath.tan(in1 * 0.5 * FastMath.PI);
282 x2 = FastMath.tan(in2 * 0.5 * FastMath.PI);
283 }
284
285 // initial estimates for the tof
286 final double tof1 = timeOfFlight(in1, longway, m, minSma, speri, chord);
287 final double tof2 = timeOfFlight(in2, longway, m, minSma, speri, chord);
288
289 // initial bounds for y
290 double y1;
291 double y2;
292 if (m == 0) {
293 y1 = FastMath.log(tof1) - logt;
294 y2 = FastMath.log(tof2) - logt;
295 } else {
296 y1 = tof1 - rtof;
297 y2 = tof2 - rtof;
298 }
299
300 // Solve for x with the secant method
301 double err = 1e20;
302 int iterations = 0;
303 final double tol = 1e-13;
304 final int maxiter = 50;
305 double xnew = 0;
306 while (err > tol && iterations < maxiter) {
307 // new x
308 xnew = (x1 * y2 - y1 * x2) / (y2 - y1);
309
310 // evaluate new time of flight
311 final double x;
312 if (m == 0) {
313 x = FastMath.exp(xnew) - 1;
314 } else {
315 x = FastMath.atan(xnew) * 2 / FastMath.PI;
316 }
317
318 final double tof = timeOfFlight(x, longway, m, minSma, speri, chord);
319
320 // new value of y
321 final double ynew;
322 if (m == 0) {
323 ynew = FastMath.log(tof) - logt;
324 } else {
325 ynew = tof - rtof;
326 }
327
328 // save previous and current values for the next iteration
329 x1 = x2;
330 x2 = xnew;
331 y1 = y2;
332 y2 = ynew;
333
334 // update error
335 err = FastMath.abs(x1 - xnew);
336
337 // increment number of iterations
338 ++iterations;
339 }
340
341 // failure to converge
342 if (err > tol) {
343 return false;
344 }
345
346 // convert converged value of x
347 final double x;
348 if (m == 0) {
349 x = FastMath.exp(xnew) - 1;
350 } else {
351 x = FastMath.atan(xnew) * 2 / FastMath.PI;
352 }
353
354 // Solution for the semi-major axis (Eq. 7.20)
355 final double sma = minSma / (1 - x * x);
356
357 // compute velocities
358 final double eta;
359 if (x < 1) {
360 // ellipse, Eqs. 7.7, 7.17
361 final double alfa = 2 * FastMath.acos(x);
362 final double beta = longway * 2 * FastMath.asin(FastMath.sqrt((speri - chord) / (2. * sma)));
363 final double psi = (alfa - beta) / 2;
364 // Eq. 7.21
365 final double sinPsi = FastMath.sin(psi);
366 final double etaSq = 2 * sma * sinPsi * sinPsi / speri;
367 eta = FastMath.sqrt(etaSq);
368 } else {
369 // hyperbola
370 final double gamma = 2 * FastMath.acosh(x);
371 final double delta = longway * 2 * FastMath.asinh(FastMath.sqrt((chord - speri) / (2 * sma)));
372 //
373 final double psi = (gamma - delta) / 2.;
374 final double sinhPsi = FastMath.sinh(psi);
375 final double etaSq = -2 * sma * sinhPsi * sinhPsi / speri;
376 eta = FastMath.sqrt(etaSq);
377 }
378
379 // radial and tangential directions for departure velocity (Eq. 7.36)
380 final double VR1 = (1. / eta) * FastMath.sqrt(1. / minSma) * (2 * lambda * minSma / r1 - (lambda + x * eta));
381 final double VT1 = (1. / eta) * FastMath.sqrt(1. / minSma) * FastMath.sqrt(r2 / r1) * FastMath.sin(dth / 2);
382 V1[0] = VR1;
383 V1[1] = VT1;
384
385 return true;
386 }
387
388 /** Compute the time of flight of a given arc of orbit.
389 * The time of flight is evaluated via the Lagrange expression.
390 *
391 * @param x x
392 * @param longway solution number; the long way or the short war
393 * @param mrev number of revolutions of the arc of orbit
394 * @param minSma minimum possible semi-major axis
395 * @param speri semi-parameter of the arc of orbit
396 * @param chord chord of the arc of orbit
397 * @return the time of flight for the given arc of orbit
398 */
399 private double timeOfFlight(final double x, final int longway, final int mrev, final double minSma,
400 final double speri, final double chord) {
401
402 final double a = minSma / (1 - x * x);
403
404 final double tof;
405 if (FastMath.abs(x) < 1) {
406 // Lagrange form of the time of flight equation Eq. (7.9)
407 // elliptical orbit (note: mu = 1)
408 final double beta = longway * 2 * FastMath.asin(FastMath.sqrt((speri - chord) / (2. * a)));
409 final double alfa = 2 * FastMath.acos(x);
410 tof = a * FastMath.sqrt(a) * ((alfa - FastMath.sin(alfa)) - (beta - FastMath.sin(beta)) + 2 * FastMath.PI * mrev);
411 } else {
412 // hyperbolic orbit
413 final double alfa = 2 * FastMath.acosh(x);
414 final double beta = longway * 2 * FastMath.asinh(FastMath.sqrt((speri - chord) / (-2. * a)));
415 tof = -a * FastMath.sqrt(-a) * ((FastMath.sinh(alfa) - alfa) - (FastMath.sinh(beta) - beta));
416 }
417
418 return tof;
419 }
420 }