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