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