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