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