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