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.estimation.measurements.PV;
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   * Gibbs initial orbit determination.
29   * An orbit is determined from three position vectors.
30   *
31   * Reference:
32   *  Vallado, D., Fundamentals of Astrodynamics and Applications
33   *
34   * @author Joris Olympio
35   * @since 8.0
36   *
37   */
38  public class IodGibbs {
39  
40      /** Threshold for checking coplanr vectors. */
41      private static final double COPLANAR_THRESHOLD = FastMath.toRadians(5.);
42  
43      /** gravitationnal constant. */
44      private final double mu;
45  
46      /** Creator.
47       *
48       * @param mu gravitational constant
49       */
50      public IodGibbs(final double mu) {
51          this.mu = mu;
52      }
53  
54      /** Give an initial orbit estimation, assuming Keplerian motion.
55       * All observations should be from the same location.
56       *
57       * @param frame measure frame
58       * @param pv1 PV measure 1 taken in frame
59       * @param pv2 PV measure 2 taken in frame
60       * @param pv3 PV measure 3 taken in frame
61       * @return an initial orbit estimation
62       */
63      public KeplerianOrbit estimate(final Frame frame, final PV pv1, final PV pv2, final PV pv3) {
64  
65          return estimate(frame,
66                          pv1.getPosition(), pv1.getDate(),
67                          pv2.getPosition(), pv2.getDate(),
68                          pv3.getPosition(), pv3.getDate());
69      }
70  
71      /** Give an initial orbit estimation, assuming Keplerian motion.
72       * All observations should be from the same location.
73       *
74       * @param frame measure frame
75       * @param r1 position 1 measured in frame
76       * @param date1 date of measure 1
77       * @param r2 position 2 measured in frame
78       * @param date2 date of measure 2
79       * @param r3 position 3 measured in frame
80       * @param date3 date of measure 3
81       * @return an initial orbit estimation
82       */
83      public KeplerianOrbit estimate(final Frame frame,
84                                     final Vector3D r1, final AbsoluteDate date1,
85                                     final Vector3D r2, final AbsoluteDate date2,
86                                     final Vector3D r3, final AbsoluteDate date3) {
87          // Checks measures are not at the same date
88          if (date1.equals(date2) || date1.equals(date3) || date2.equals(date3)) {
89              //throw new OrekitException("The measures are not different!");
90          }
91  
92          // Checks measures are in the same plane
93          final double num = r1.normalize().dotProduct(r2.normalize().crossProduct(r3.normalize()));
94          final double alpha = FastMath.PI / 2.0 - FastMath.acos(num);
95          if (FastMath.abs(alpha) > COPLANAR_THRESHOLD) {
96              // throw something
97              //throw new OrekitException("Non coplanar points!");
98          }
99  
100         final Vector3D D = r1.crossProduct(r2).add(r2.crossProduct(r3).add(r3.crossProduct(r1)));
101 
102         final Vector3D N = (r2.crossProduct(r3)).scalarMultiply(r1.getNorm())
103                 .add((r3.crossProduct(r1)).scalarMultiply(r2.getNorm()))
104                 .add((r1.crossProduct(r2)).scalarMultiply(r3.getNorm()));
105 
106         final Vector3D B = D.crossProduct(r2);
107 
108         final Vector3D S = r1.scalarMultiply(r2.getNorm() - r3.getNorm())
109                 .add(r2.scalarMultiply(r3.getNorm() - r1.getNorm())
110                      .add(r3.scalarMultiply(r1.getNorm() - r2.getNorm())));
111 
112         // middle velocity
113         final double vm = FastMath.sqrt(mu / (N.getNorm() * D.getNorm()));
114         final Vector3D vlEci = B.scalarMultiply(vm / r2.getNorm()).add(S.scalarMultiply(vm));
115 
116         // compile a new middle point with position, velocity
117         final PVCoordinates pv = new PVCoordinates(r2, vlEci);
118         final AbsoluteDate date = date2;
119 
120         // compute the equivalent Keplerian orbit
121         return new KeplerianOrbit(pv, frame, date, mu);
122     }
123 }