1 /* Copyright 2002-2024 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.estimation.measurements.AngularAzEl;
22 import org.orekit.estimation.measurements.AngularRaDec;
23 import org.orekit.frames.Frame;
24 import org.orekit.orbits.CartesianOrbit;
25 import org.orekit.orbits.Orbit;
26 import org.orekit.time.AbsoluteDate;
27 import org.orekit.utils.PVCoordinates;
28
29 /**
30 * Gooding angles only Initial Orbit Determination (IOD) algorithm, assuming Keplerian motion.
31 * <p>
32 * An orbit is determined from three lines of sight w.r.t. their respective observers
33 * inertial positions vectors. Gooding algorithm can handle multiple satellite's revolutions.
34 *
35 * Reference:
36 * Gooding, R.H., A New Procedure for Orbit Determination Based on Three Lines of Sight (Angles only),
37 * Technical Report 93004, April 1993
38 * </p>
39 * @author Joris Olympio
40 * @since 8.0
41 */
42 public class IodGooding {
43
44 /** Gravitationnal constant. */
45 private final double mu;
46
47 /** Normalizing constant for distances. */
48 private double R;
49
50 /** Normalizing constant for velocities. */
51 private double V;
52
53 /** Normalizing constant for duration. */
54 private double T;
55
56 /** observer position 1. */
57 private Vector3D vObserverPosition1;
58
59 /** observer position 2. */
60 private Vector3D vObserverPosition2;
61
62 /** observer position 3. */
63 private Vector3D vObserverPosition3;
64
65 /** Date of the first observation. * */
66 private AbsoluteDate date1;
67
68 /** Radius of point 1 (X-R1). */
69 private double R1;
70 /** Radius of point 2 (X-R2). */
71 private double R2;
72 /** Radius of point 3 (X-R3). */
73 private double R3;
74
75 /** Range of point 1 (O1-R1). */
76 private double rho1;
77
78 /** Range of point 2 (O1-R1). */
79 private double rho2;
80
81 /** Range of point 1 (O1-R1). */
82 private double rho3;
83
84 /** working variable. */
85 private double D1;
86
87 /** working variable. */
88 private double D3;
89
90 /** factor for FD. */
91 private double facFiniteDiff;
92
93 /** Simple Lambert's problem solver. */
94 private IodLambert lambert;
95
96 /**
97 * Constructor.
98 *
99 * @param mu gravitational constant
100 */
101 public IodGooding(final double mu) {
102 this.mu = mu;
103
104 this.rho1 = 0;
105 this.rho2 = 0;
106 this.rho3 = 0;
107 }
108
109 /** Get range for observation (1).
110 *
111 * @return the range for observation (1)
112 */
113 public double getRange1() {
114 return rho1 * R;
115 }
116
117 /** Get range for observation (2).
118 *
119 * @return the range for observation (2)
120 */
121 public double getRange2() {
122 return rho2 * R;
123 }
124
125 /** Get range for observation (3).
126 *
127 * @return the range for observation (3)
128 */
129 public double getRange3() {
130 return rho3 * R;
131 }
132
133 /** Estimate orbit from three angular observations.
134 * <p>
135 * This signature assumes there was less than an half revolution between start and final date
136 * </p>
137 * @param outputFrame inertial frame for observer coordinates and orbit estimate
138 * @param azEl1 first angular observation
139 * @param azEl2 second angular observation
140 * @param azEl3 third angular observation
141 * @param rho1init initial guess of the range problem. range 1, in meters
142 * @param rho3init initial guess of the range problem. range 3, in meters
143 * @return an estimate of the Keplerian orbit at the central date
144 * (i.e., date of the second angular observation)
145 * @since 12.0
146 */
147 public Orbit estimate(final Frame outputFrame, final AngularAzEl azEl1,
148 final AngularAzEl azEl2, final AngularAzEl azEl3,
149 final double rho1init, final double rho3init) {
150 return estimate(outputFrame, azEl1, azEl2, azEl3, rho1init, rho3init, 0, true);
151 }
152
153 /** Estimate orbit from three angular observations.
154 *
155 * @param outputFrame inertial frame for observer coordinates and orbit estimate
156 * @param azEl1 first angular observation
157 * @param azEl2 second angular observation
158 * @param azEl3 third angular observation
159 * @param rho1init initial guess of the range problem. range 1, in meters
160 * @param rho3init initial guess of the range problem. range 3, in meters
161 * @param nRev number of complete revolutions between observation 1 and 3
162 * @param direction true if posigrade (short way)
163 * @return an estimate of the Keplerian orbit at the central date
164 * (i.e., date of the second angular observation)
165 * @since 11.0
166 */
167 public Orbit estimate(final Frame outputFrame, final AngularAzEl azEl1,
168 final AngularAzEl azEl2, final AngularAzEl azEl3,
169 final double rho1init, final double rho3init,
170 final int nRev, final boolean direction) {
171 return estimate(outputFrame,
172 azEl1.getGroundStationPosition(outputFrame),
173 azEl2.getGroundStationPosition(outputFrame),
174 azEl3.getGroundStationPosition(outputFrame),
175 azEl1.getObservedLineOfSight(outputFrame), azEl1.getDate(),
176 azEl2.getObservedLineOfSight(outputFrame), azEl2.getDate(),
177 azEl3.getObservedLineOfSight(outputFrame), azEl3.getDate(),
178 rho1init, rho3init, nRev, direction);
179 }
180
181 /** Estimate orbit from three angular observations.
182 * <p>
183 * This signature assumes there was less than an half revolution between start and final date
184 * </p>
185 * @param outputFrame inertial frame for observer coordinates and orbit estimate
186 * @param raDec1 first angular observation
187 * @param raDec2 second angular observation
188 * @param raDec3 third angular observation
189 * @param rho1init initial guess of the range problem. range 1, in meters
190 * @param rho3init initial guess of the range problem. range 3, in meters
191 * @return an estimate of the Keplerian orbit at the central date
192 * (i.e., date of the second angular observation)
193 * @since 11.0
194 */
195 public Orbit estimate(final Frame outputFrame, final AngularRaDec raDec1,
196 final AngularRaDec raDec2, final AngularRaDec raDec3,
197 final double rho1init, final double rho3init) {
198 return estimate(outputFrame, raDec1, raDec2, raDec3, rho1init, rho3init, 0, true);
199 }
200
201 /** Estimate orbit from three angular observations.
202 *
203 * @param outputFrame inertial frame for observer coordinates and orbit estimate
204 * @param raDec1 first angular observation
205 * @param raDec2 second angular observation
206 * @param raDec3 third angular observation
207 * @param rho1init initial guess of the range problem. range 1, in meters
208 * @param rho3init initial guess of the range problem. range 3, in meters
209 * @param nRev number of complete revolutions between observation 1 and 3
210 * @param direction true if posigrade (short way)
211 * @return an estimate of the Keplerian orbit at the central date
212 * (i.e., date of the second angular observation)
213 * @since 11.0
214 */
215 public Orbit estimate(final Frame outputFrame, final AngularRaDec raDec1,
216 final AngularRaDec raDec2, final AngularRaDec raDec3,
217 final double rho1init, final double rho3init,
218 final int nRev, final boolean direction) {
219 return estimate(outputFrame,
220 raDec1.getGroundStationPosition(outputFrame),
221 raDec2.getGroundStationPosition(outputFrame),
222 raDec3.getGroundStationPosition(outputFrame),
223 raDec1.getObservedLineOfSight(outputFrame), raDec1.getDate(),
224 raDec2.getObservedLineOfSight(outputFrame), raDec2.getDate(),
225 raDec3.getObservedLineOfSight(outputFrame), raDec3.getDate(),
226 rho1init, rho3init, nRev, direction);
227 }
228
229 /** Estimate orbit from three line of sight.
230 * <p>
231 * This signature assumes there was less than an half revolution between start and final date
232 * </p>
233 * @param outputFrame inertial frame for observer coordinates and orbit estimate
234 * @param O1 Observer position 1
235 * @param O2 Observer position 2
236 * @param O3 Observer position 3
237 * @param lineOfSight1 line of sight 1
238 * @param dateObs1 date of observation 1
239 * @param lineOfSight2 line of sight 2
240 * @param dateObs2 date of observation 1
241 * @param lineOfSight3 line of sight 3
242 * @param dateObs3 date of observation 1
243 * @param rho1init initial guess of the range problem. range 1, in meters
244 * @param rho3init initial guess of the range problem. range 3, in meters
245 * @return an estimate of the Keplerian orbit at the central date
246 * (i.e., date of the second angular observation)
247 */
248 public Orbit estimate(final Frame outputFrame, final Vector3D O1, final Vector3D O2, final Vector3D O3,
249 final Vector3D lineOfSight1, final AbsoluteDate dateObs1,
250 final Vector3D lineOfSight2, final AbsoluteDate dateObs2,
251 final Vector3D lineOfSight3, final AbsoluteDate dateObs3,
252 final double rho1init, final double rho3init) {
253 return this.estimate(outputFrame, O1, O2, O3, lineOfSight1, dateObs1, lineOfSight2, dateObs2,
254 lineOfSight3, dateObs3, rho1init, rho3init, 0, true);
255 }
256
257 /** Estimate orbit from three line of sight.
258 *
259 * @param outputFrame inertial frame for observer coordinates and orbit estimate
260 * @param O1 Observer position 1
261 * @param O2 Observer position 2
262 * @param O3 Observer position 3
263 * @param lineOfSight1 line of sight 1
264 * @param dateObs1 date of observation 1
265 * @param lineOfSight2 line of sight 2
266 * @param dateObs2 date of observation 2
267 * @param lineOfSight3 line of sight 3
268 * @param dateObs3 date of observation 3
269 * @param rho1init initial guess of the range problem. range 1, in meters
270 * @param rho3init initial guess of the range problem. range 3, in meters
271 * @param nRev number of complete revolutions between observation1 and 3
272 * @param direction true if posigrade (short way)
273 * @return an estimate of the Keplerian orbit at the central date
274 * (i.e., date of the second angular observation)
275 */
276 public Orbit estimate(final Frame outputFrame,
277 final Vector3D O1, final Vector3D O2, final Vector3D O3,
278 final Vector3D lineOfSight1, final AbsoluteDate dateObs1,
279 final Vector3D lineOfSight2, final AbsoluteDate dateObs2,
280 final Vector3D lineOfSight3, final AbsoluteDate dateObs3,
281 final double rho1init, final double rho3init, final int nRev,
282 final boolean direction) {
283
284 this.date1 = dateObs1;
285
286 // normalizing coefficients
287 R = FastMath.max(rho1init, rho3init);
288 V = FastMath.sqrt(mu / R);
289 T = R / V;
290
291 // Initialize Lambert's problem solver for non-dimensional units.
292 lambert = new IodLambert(1.);
293
294 this.vObserverPosition1 = O1.scalarMultiply(1. / R);
295 this.vObserverPosition2 = O2.scalarMultiply(1. / R);
296 this.vObserverPosition3 = O3.scalarMultiply(1. / R);
297
298 // solve the range problem
299 solveRangeProblem(outputFrame,
300 rho1init / R, rho3init / R,
301 dateObs3.durationFrom(dateObs1) / T, dateObs2.durationFrom(dateObs1) / T,
302 nRev,
303 direction,
304 lineOfSight1, lineOfSight2, lineOfSight3);
305
306 // use the Gibbs problem to get the orbit now that we have three position vectors.
307 final IodGibbs gibbs = new IodGibbs(mu);
308 final Vector3D p1 = vObserverPosition1.add(lineOfSight1.scalarMultiply(rho1)).scalarMultiply(R);
309 final Vector3D p2 = vObserverPosition2.add(lineOfSight2.scalarMultiply(rho2)).scalarMultiply(R);
310 final Vector3D p3 = vObserverPosition3.add(lineOfSight3.scalarMultiply(rho3)).scalarMultiply(R);
311 return gibbs.estimate(outputFrame, p1, dateObs1, p2, dateObs2, p3, dateObs3);
312 }
313
314 /** Solve the range problem when three line of sight are given.
315 * @param frame frame to be used (orbit frame)
316 * @param rho1init initial value for range R1, in meters
317 * @param rho3init initial value for range R3, in meters
318 * @param T13 time of flight 1->3, in seconds
319 * @param T12 time of flight 1->2, in seconds
320 * @param nRev number of revolutions
321 * @param direction posigrade (true) or retrograde
322 * @param lineOfSight1 line of sight 1
323 * @param lineOfSight2 line of sight 2
324 * @param lineOfSight3 line of sight 3
325 */
326 private void solveRangeProblem(final Frame frame,
327 final double rho1init, final double rho3init,
328 final double T13, final double T12,
329 final int nRev,
330 final boolean direction,
331 final Vector3D lineOfSight1,
332 final Vector3D lineOfSight2,
333 final Vector3D lineOfSight3) {
334 final int maxIterations = 100;
335 final double ARBF = 1e-6; // finite differences step
336 boolean withHalley = true; // use Halley's method
337 final double cvtol = 1e-14; // convergence tolerance
338
339 rho1 = rho1init;
340 rho3 = rho3init;
341
342 int iter = 0;
343 double stoppingCriterion = 10 * cvtol;
344 while (iter < maxIterations && FastMath.abs(stoppingCriterion) > cvtol) {
345 facFiniteDiff = ARBF;
346
347 // proposed in the original algorithm by Gooding.
348 // We change the threshold to maxIterations / 2.
349 if (iter >= (maxIterations / 2)) {
350 withHalley = true;
351 }
352
353 // tentative position for R2
354 final Vector3D P2 = getPositionOnLoS2(frame,
355 lineOfSight1, rho1,
356 lineOfSight3, rho3,
357 T13, T12, nRev, direction);
358
359 if (P2 == null) {
360 modifyIterate(lineOfSight1, lineOfSight3);
361 } else {
362 //
363 R2 = P2.getNorm();
364
365 // compute current line of sight for (2) and the associated range
366 final Vector3D C = P2.subtract(vObserverPosition2);
367 rho2 = C.getNorm();
368 // indicate how close we are to the direction of sight for measurement (2)
369 // for convergence test
370 final double CR = lineOfSight2.dotProduct(C);
371
372 // construct a basis and the function f and g.
373 // Specifically, f is the P-coordinate and g the N-coordinate.
374 // They should be zero when line of sight 2 and current direction for 2 from O2 are aligned.
375 final Vector3D u = lineOfSight2.crossProduct(C);
376 final Vector3D P = (u.crossProduct(lineOfSight2)).normalize();
377 final Vector3D ENt = lineOfSight2.crossProduct(P);
378
379 // if ENt is zero we have a solution!
380 final double ENR = ENt.getNorm();
381 if (ENR == 0.) {
382 break;
383 }
384
385 //Normalize EN
386 final Vector3D EN = ENt.normalize();
387
388 // Coordinate along 'F function'
389 final double Fc = P.dotProduct(C);
390 //Gc = EN.dotProduct(C);
391
392 // Now get partials, by finite differences
393 final double[] FD = new double[2];
394 final double[] GD = new double[2];
395 computeDerivatives(frame,
396 rho1, rho3,
397 lineOfSight1, lineOfSight3,
398 P, EN,
399 Fc,
400 T13, T12,
401 withHalley,
402 nRev,
403 direction,
404 FD, GD);
405
406 // terms of the Jacobian
407 final double fr1 = FD[0];
408 final double fr3 = FD[1];
409 final double gr1 = GD[0];
410 final double gr3 = GD[1];
411 // determinant of the Jacobian matrix
412 final double detj = fr1 * gr3 - fr3 * gr1;
413
414 // compute the Newton step
415 D3 = -gr3 * Fc / detj;
416 D1 = gr1 * Fc / detj;
417
418 // update current values of ranges
419 rho1 = rho1 + D3;
420 rho3 = rho3 + D1;
421
422 // convergence tests
423 final double den = FastMath.max(CR, R2);
424 stoppingCriterion = Fc / den;
425 }
426
427 ++iter;
428 } // end while
429 }
430
431 /** Change the current iterate if Lambert's problem solver failed to find a solution.
432 *
433 * @param lineOfSight1 line of sight 1
434 * @param lineOfSight3 line of sight 3
435 */
436 private void modifyIterate(final Vector3D lineOfSight1,
437 final Vector3D lineOfSight3) {
438 // This is a modifier proposed in the initial paper of Gooding.
439 // Try to avoid Lambert-fail, by common-perpendicular starters
440 final Vector3D R13 = vObserverPosition3.subtract(vObserverPosition1);
441 D1 = R13.dotProduct(lineOfSight1);
442 D3 = R13.dotProduct(lineOfSight3);
443 final double D2 = lineOfSight1.dotProduct(lineOfSight3);
444 final double D4 = 1. - D2 * D2;
445 rho1 = FastMath.max((D1 - D3 * D2) / D4, 0.);
446 rho3 = FastMath.max((D1 * D2 - D3) / D4, 0.);
447 }
448
449 /** Compute the derivatives by finite-differences for the range problem.
450 * Specifically, we are trying to solve the problem:
451 * f(x, y) = 0
452 * g(x, y) = 0
453 * So, in a Newton-Raphson process, we would need the derivatives:
454 * fx, fy, gx, gy
455 * Enventually,
456 * dx =-f*gy / D
457 * dy = f*gx / D
458 * where D is the determinant of the Jacobian matrix.
459 *
460 * @param frame frame to be used (orbit frame)
461 * @param x current range 1
462 * @param y current range 3
463 * @param lineOfSight1 line of sight
464 * @param lineOfSight3 line of sight
465 * @param Pin basis vector
466 * @param Ein basis vector
467 * @param F value of the f-function
468 * @param T13 time of flight 1->3, in seconds
469 * @param T12 time of flight 1->2, in seconds
470 * @param withHalley use Halley iterative method
471 * @param nRev number of revolutions
472 * @param direction direction of motion
473 * @param FD derivatives of f wrt (rho1, rho3) by finite differences
474 * @param GD derivatives of g wrt (rho1, rho3) by finite differences
475 */
476 private void computeDerivatives(final Frame frame,
477 final double x, final double y,
478 final Vector3D lineOfSight1, final Vector3D lineOfSight3,
479 final Vector3D Pin,
480 final Vector3D Ein,
481 final double F,
482 final double T13, final double T12,
483 final boolean withHalley,
484 final int nRev,
485 final boolean direction,
486 final double[] FD, final double[] GD) {
487
488 final Vector3D P = Pin.normalize();
489 final Vector3D EN = Ein.normalize();
490
491 // Now get partials, by finite differences
492 // steps for the differentiation
493 final double dx = facFiniteDiff * x;
494 final double dy = facFiniteDiff * y;
495
496 final Vector3D Cm1 = getPositionOnLoS2 (frame,
497 lineOfSight1, x - dx,
498 lineOfSight3, y,
499 T13, T12, nRev, direction).subtract(vObserverPosition2);
500
501 final double Fm1 = P.dotProduct(Cm1);
502 final double Gm1 = EN.dotProduct(Cm1);
503
504 final Vector3D Cp1 = getPositionOnLoS2 (frame,
505 lineOfSight1, x + dx,
506 lineOfSight3, y,
507 T13, T12, nRev, direction).subtract(vObserverPosition2);
508
509 final double Fp1 = P.dotProduct(Cp1);
510 final double Gp1 = EN.dotProduct(Cp1);
511
512 // derivatives df/drho1 and dg/drho1
513 final double Fx = (Fp1 - Fm1) / (2. * dx);
514 final double Gx = (Gp1 - Gm1) / (2. * dx);
515
516 final Vector3D Cm3 = getPositionOnLoS2 (frame,
517 lineOfSight1, x,
518 lineOfSight3, y - dy,
519 T13, T12, nRev, direction).subtract(vObserverPosition2);
520
521 final double Fm3 = P.dotProduct(Cm3);
522 final double Gm3 = EN.dotProduct(Cm3);
523
524 final Vector3D Cp3 = getPositionOnLoS2 (frame,
525 lineOfSight1, x,
526 lineOfSight3, y + dy,
527 T13, T12, nRev, direction).subtract(vObserverPosition2);
528
529 final double Fp3 = P.dotProduct(Cp3);
530 final double Gp3 = EN.dotProduct(Cp3);
531
532 // derivatives df/drho3 and dg/drho3
533 final double Fy = (Fp3 - Fm3) / (2. * dy);
534 final double Gy = (Gp3 - Gm3) / (2. * dy);
535 final double detJac = Fx * Gy - Fy * Gx;
536
537 // Coefficients for the classical Newton-Raphson iterative method
538 FD[0] = Fx;
539 FD[1] = Fy;
540 GD[0] = Gx;
541 GD[1] = Gy;
542
543 // Modified Newton-Raphson process, with Halley's method to have cubic convergence.
544 // This requires computing second order derivatives.
545 if (withHalley) {
546 //
547 final double hrho1Sq = dx * dx;
548 final double hrho3Sq = dy * dy;
549
550 // Second order derivatives: d^2f / drho1^2 and d^2g / drho3^2
551 final double Fxx = (Fp1 + Fm1 - 2 * F) / hrho1Sq;
552 final double Gxx = (Gp1 + Gm1 - 2 * F) / hrho1Sq;
553 final double Fyy = (Fp3 + Fp3 - 2 * F) / hrho3Sq;
554 final double Gyy = (Gm3 + Gm3 - 2 * F) / hrho3Sq;
555
556 final Vector3D Cp13 = getPositionOnLoS2 (frame,
557 lineOfSight1, x + dx,
558 lineOfSight3, y + dy,
559 T13, T12, nRev, direction).subtract(vObserverPosition2);
560
561 // f function value at (x1+dx1, x3+dx3)
562 final double Fp13 = P.dotProduct(Cp13);
563 // g function value at (x1+dx1, x3+dx3)
564 final double Gp13 = EN.dotProduct(Cp13);
565
566 final Vector3D Cm13 = getPositionOnLoS2 (frame,
567 lineOfSight1, x - dx,
568 lineOfSight3, y - dy,
569 T13, T12, nRev, direction).subtract(vObserverPosition2);
570
571 // f function value at (x1-dx1, x3-dx3)
572 final double Fm13 = P.dotProduct(Cm13);
573 // g function value at (x1-dx1, x3-dx3)
574 final double Gm13 = EN.dotProduct(Cm13);
575
576 // Second order derivatives:
577 final double Fxy = (Fp13 + Fm13) / (2 * dx * dy) - 0.5 * (Fxx * dx / dy + Fyy * dy / dx) - F / (dx * dy);
578 final double Gxy = (Gp13 + Gm13) / (2 * dx * dy) - 0.5 * (Gxx * dx / dy + Gyy * dy / dx) - F / (dx * dy);
579
580 // delta Newton Raphson, 1st order step
581 final double dx3NR = -Gy * F / detJac;
582 final double dx1NR = Gx * F / detJac;
583
584 // terms of the Jacobian, considering the development, after linearization
585 // of the second order Taylor expansion around the Newton Raphson iterate:
586 // (fx + 1/2 * fxx * dx* + 1/2 * fxy * dy*) * dx
587 // + (fy + 1/2 * fyy * dy* + 1/2 * fxy * dx*) * dy
588 // where: dx* and dy* would be the step of the Newton raphson process.
589 final double FxH = Fx + 0.5 * (Fxx * dx3NR + Fxy * dx1NR);
590 final double FyH = Fy + 0.5 * (Fxy * dx3NR + Fxx * dx1NR);
591 final double GxH = Gx + 0.5 * (Gxx * dx3NR + Gxy * dx1NR);
592 final double GyH = Gy + 0.5 * (Gxy * dx3NR + Gyy * dx1NR);
593
594 // New Halley's method "Jacobian"
595 FD[0] = FxH;
596 FD[1] = FyH;
597 GD[0] = GxH;
598 GD[1] = GyH;
599 }
600 }
601
602 /** Calculate the position along sight-line.
603 * @param frame frame to be used (orbit frame)
604 * @param E1 line of sight 1
605 * @param RO1 distance along E1
606 * @param E3 line of sight 3
607 * @param RO3 distance along E3
608 * @param T12 time of flight
609 * @param T13 time of flight
610 * @param nRev number of revolutions
611 * @param posigrade direction of motion
612 * @return (R2-O2)
613 */
614 private Vector3D getPositionOnLoS2(final Frame frame,
615 final Vector3D E1, final double RO1,
616 final Vector3D E3, final double RO3,
617 final double T13, final double T12,
618 final int nRev, final boolean posigrade) {
619 final Vector3D P1 = vObserverPosition1.add(E1.scalarMultiply(RO1));
620 R1 = P1.getNorm();
621
622 final Vector3D P3 = vObserverPosition3.add(E3.scalarMultiply(RO3));
623 R3 = P3.getNorm();
624
625 final Vector3D P13 = P1.crossProduct(P3);
626
627 // sweep angle
628 // (Fails only if either R1 or R2 is zero)
629 double TH = FastMath.atan2(P13.getNorm(), P1.dotProduct(P3));
630
631 // compute the number of revolutions
632 if (!posigrade) {
633 TH = 2 * FastMath.PI - TH;
634 }
635
636 // Solve the Lambert's problem to get the velocities at endpoints
637 final double[] V1 = new double[2];
638 // work with non-dimensional units (MU=1)
639 final boolean exitflag = lambert.solveLambertPb(R1, R3, TH, T13, nRev, V1);
640
641 if (exitflag) {
642 // basis vectors
643 final Vector3D Pn = P1.crossProduct(P3);
644 final Vector3D Pt = Pn.crossProduct(P1);
645
646 // tangential velocity norm
647 double RT = Pt.getNorm();
648 if (!posigrade) {
649 RT = -RT;
650 }
651
652 // velocity vector at P1
653 final Vector3D Vel1 = new Vector3D(V1[0] / R1, P1, V1[1] / RT, Pt);
654
655 // estimate the position at the second observation time
656 // propagate (P1, V1) during TAU + T12 to get (P2, V2)
657 final PVCoordinates pv1 = new PVCoordinates(P1, Vel1);
658 final Orbit orbit = new CartesianOrbit(pv1, frame, date1, 1.);
659
660 return orbit.shiftedBy(T12).getPosition();
661 }
662
663 return null;
664 }
665
666 }