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.analysis.solvers.LaguerreSolver;
20 import org.hipparchus.complex.Complex;
21 import org.hipparchus.geometry.euclidean.threed.Vector3D;
22 import org.hipparchus.linear.Array2DRowRealMatrix;
23 import org.hipparchus.linear.LUDecomposition;
24 import org.hipparchus.util.FastMath;
25 import org.orekit.estimation.measurements.AngularRaDec;
26 import org.orekit.frames.Frame;
27 import org.orekit.orbits.CartesianOrbit;
28 import org.orekit.time.AbsoluteDate;
29 import org.orekit.utils.PVCoordinates;
30
31 /**
32 * Laplace angles-only initial orbit determination, assuming Keplerian motion.
33 * An orbit is determined from three angular observations from the same site.
34 *
35 *
36 * Reference:
37 * Bate, R., Mueller, D. D., & White, J. E. (1971). Fundamentals of astrodynamics.
38 * New York: Dover Publications.
39 *
40 * @author Shiva Iyer
41 * @since 10.1
42 */
43 public class IodLaplace {
44
45 /** Gravitational constant. */
46 private final double mu;
47
48 /** Constructor.
49 *
50 * @param mu gravitational constant
51 */
52 public IodLaplace(final double mu) {
53 this.mu = mu;
54 }
55
56 /** Estimate the orbit from three angular observations at the same location.
57 *
58 * @param frame inertial frame for observer coordinates and orbit estimate
59 * @param obsPva Observer coordinates at time of raDec2
60 * @param raDec1 first angular observation
61 * @param raDec2 second angular observation
62 * @param raDec3 third angular observation
63 * @return estimate of the orbit at the central date or null if
64 * no estimate is possible with the given data
65 * @since 11.0
66 */
67 public CartesianOrbit estimate(final Frame frame, final PVCoordinates obsPva,
68 final AngularRaDec raDec1, final AngularRaDec raDec2,
69 final AngularRaDec raDec3) {
70 return estimate(frame, obsPva,
71 raDec1.getDate(), lineOfSight(raDec1),
72 raDec2.getDate(), lineOfSight(raDec2),
73 raDec3.getDate(), lineOfSight(raDec3));
74 }
75
76 /** Estimate orbit from three line of sight angles from the same location.
77 *
78 * @param frame inertial frame for observer coordinates and orbit estimate
79 * @param obsPva Observer coordinates at time obsDate2
80 * @param obsDate1 date of observation 1
81 * @param los1 line of sight unit vector 1
82 * @param obsDate2 date of observation 2
83 * @param los2 line of sight unit vector 2
84 * @param obsDate3 date of observation 3
85 * @param los3 line of sight unit vector 3
86 * @return estimate of the orbit at the central date dateObs2 or null if
87 * no estimate is possible with the given data
88 */
89 public CartesianOrbit estimate(final Frame frame, final PVCoordinates obsPva,
90 final AbsoluteDate obsDate1, final Vector3D los1,
91 final AbsoluteDate obsDate2, final Vector3D los2,
92 final AbsoluteDate obsDate3, final Vector3D los3) {
93 // The first observation is taken as t1 = 0
94 final double t2 = obsDate2.durationFrom(obsDate1);
95 final double t3 = obsDate3.durationFrom(obsDate1);
96
97 // Calculate the first and second derivatives of the Line Of Sight vector at t2
98 final Vector3D Ldot = los1.scalarMultiply((t2 - t3) / (t2 * t3)).
99 add(los2.scalarMultiply((2.0 * t2 - t3) / (t2 * (t2 - t3)))).
100 add(los3.scalarMultiply(t2 / (t3 * (t3 - t2))));
101 final Vector3D Ldotdot = los1.scalarMultiply(2.0 / (t2 * t3)).
102 add(los2.scalarMultiply(2.0 / (t2 * (t2 - t3)))).
103 add(los3.scalarMultiply(2.0 / (t3 * (t3 - t2))));
104
105 // The determinant will vanish if the observer lies in the plane of the orbit at t2
106 final double D = 2.0 * getDeterminant(los2, Ldot, Ldotdot);
107 if (FastMath.abs(D) < 1.0E-14) {
108 return null;
109 }
110
111 final double Dsq = D * D;
112 final double R = obsPva.getPosition().getNorm();
113 final double RdotL = obsPva.getPosition().dotProduct(los2);
114
115 final double D1 = getDeterminant(los2, Ldot, obsPva.getAcceleration());
116 final double D2 = getDeterminant(los2, Ldot, obsPva.getPosition());
117
118 // Coefficients of the 8th order polynomial we need to solve to determine "r"
119 final double[] coeff = new double[] {-4.0 * mu * mu * D2 * D2 / Dsq,
120 0.0,
121 0.0,
122 4.0 * mu * D2 * (RdotL / D - 2.0 * D1 / Dsq),
123 0.0,
124 0.0,
125 4.0 * D1 * RdotL / D - 4.0 * D1 * D1 / Dsq - R * R, 0.0,
126 1.0};
127
128 // Use the Laguerre polynomial solver and take the initial guess to be
129 // 5 times the observer's position magnitude
130 final LaguerreSolver solver = new LaguerreSolver(1E-10, 1E-10, 1E-10);
131 final Complex[] roots = solver.solveAllComplex(coeff, 5.0 * R);
132
133 // We consider "r" to be the positive real root with the largest magnitude
134 double rMag = 0.0;
135 for (int i = 0; i < roots.length; i++) {
136 if (roots[i].getReal() > rMag &&
137 FastMath.abs(roots[i].getImaginary()) < solver.getAbsoluteAccuracy()) {
138 rMag = roots[i].getReal();
139 }
140 }
141 if (rMag == 0.0) {
142 return null;
143 }
144
145 // Calculate rho, the slant range from the observer to the satellite at t2.
146 // This yields the "r" vector, which is the satellite's position vector at t2.
147 final double rCubed = rMag * rMag * rMag;
148 final double rho = -2.0 * D1 / D - 2.0 * mu * D2 / (D * rCubed);
149 final Vector3D posVec = los2.scalarMultiply(rho).add(obsPva.getPosition());
150
151 // Calculate rho_dot at t2, which will yield the satellite's velocity vector at t2
152 final double D3 = getDeterminant(los2, obsPva.getAcceleration(), Ldotdot);
153 final double D4 = getDeterminant(los2, obsPva.getPosition(), Ldotdot);
154 final double rhoDot = -D3 / D - mu * D4 / (D * rCubed);
155 final Vector3D velVec = los2.scalarMultiply(rhoDot).
156 add(Ldot.scalarMultiply(rho)).
157 add(obsPva.getVelocity());
158
159 // Return the estimated orbit
160 return new CartesianOrbit(new PVCoordinates(posVec, velVec), frame, obsDate2, mu);
161 }
162
163 /**
164 * Calculates the line of sight vector.
165 * @param alpha right ascension angle, in radians
166 * @param delta declination angle, in radians
167 * @return the line of sight vector
168 * @since 11.0
169 */
170 public static Vector3D lineOfSight(final double alpha, final double delta) {
171 return new Vector3D(FastMath.cos(delta) * FastMath.cos(alpha),
172 FastMath.cos(delta) * FastMath.sin(alpha),
173 FastMath.sin(delta));
174 }
175
176 /**
177 * Calculate the line of sight vector from an AngularRaDec measurement.
178 * @param raDec measurement
179 * @return the line of sight vector
180 * @since 11.0
181 */
182 public static Vector3D lineOfSight(final AngularRaDec raDec) {
183
184 // Observed values
185 final double[] observed = raDec.getObservedValue();
186
187 // Return
188 return lineOfSight(observed[0], observed[1]);
189
190 }
191
192 /** Calculate the determinant of the matrix with given column vectors.
193 *
194 * @param col0 Matrix column 0
195 * @param col1 Matrix column 1
196 * @param col2 Matrix column 2
197 * @return matrix determinant
198 *
199 */
200 private double getDeterminant(final Vector3D col0, final Vector3D col1, final Vector3D col2) {
201 final Array2DRowRealMatrix mat = new Array2DRowRealMatrix(3, 3);
202 mat.setColumn(0, col0.toArray());
203 mat.setColumn(1, col1.toArray());
204 mat.setColumn(2, col2.toArray());
205 return new LUDecomposition(mat).getDeterminant();
206 }
207
208 }