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