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