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