1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.propagation;
18
19 import org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.geometry.euclidean.threed.Vector3D;
21 import org.hipparchus.util.FastMath;
22 import org.orekit.errors.OrekitException;
23 import org.orekit.errors.OrekitMessages;
24 import org.orekit.orbits.FieldOrbit;
25 import org.orekit.orbits.Orbit;
26 import org.orekit.orbits.OrbitType;
27 import org.orekit.orbits.PositionAngleType;
28 import org.orekit.propagation.numerical.FieldNumericalPropagator;
29 import org.orekit.propagation.numerical.NumericalPropagator;
30 import org.orekit.utils.PVCoordinates;
31
32 import java.util.Arrays;
33
34
35
36
37
38
39
40
41
42
43
44
45 public interface ToleranceProvider extends CartesianToleranceProvider {
46
47
48
49
50
51
52
53
54 double[][] getTolerances(Orbit referenceOrbit, OrbitType propagationOrbitType,
55 PositionAngleType positionAngleType);
56
57
58
59
60
61
62
63 default double[][] getTolerances(Orbit referenceOrbit, OrbitType propagationOrbitType) {
64 return getTolerances(referenceOrbit, propagationOrbitType, NumericalPropagator.DEFAULT_POSITION_ANGLE_TYPE);
65 }
66
67
68
69
70
71
72
73
74
75 default <T extends CalculusFieldElement<T>> double[][] getTolerances(FieldOrbit<T> referenceOrbit,
76 OrbitType propagationOrbitType,
77 PositionAngleType positionAngleType) {
78 return getTolerances(referenceOrbit.toOrbit(), propagationOrbitType, positionAngleType);
79 }
80
81
82
83
84
85
86
87
88 default <T extends CalculusFieldElement<T>> double[][] getTolerances(FieldOrbit<T> referenceOrbit,
89 OrbitType propagationOrbitType) {
90 return getTolerances(referenceOrbit, propagationOrbitType, NumericalPropagator.DEFAULT_POSITION_ANGLE_TYPE);
91 }
92
93
94
95
96
97
98
99 static ToleranceProvider of(final double absoluteTolerance, final double relativeTolerance) {
100
101 return new ToleranceProvider() {
102 @Override
103 public double[][] getTolerances(final Orbit referenceOrbit, final OrbitType propagationOrbitType,
104 final PositionAngleType positionAngleType) {
105 return getTolerances();
106 }
107
108 @Override
109 public double[][] getTolerances(final Vector3D position, final Vector3D velocity) {
110 return getTolerances();
111 }
112
113
114
115
116
117 double[][] getTolerances() {
118 final double[] absTol = new double[7];
119 Arrays.fill(absTol, absoluteTolerance);
120 final double[] relTol = new double[absTol.length];
121 Arrays.fill(relTol, relativeTolerance);
122 return new double[][] { absTol, relTol };
123 }
124 };
125 }
126
127
128
129
130
131
132
133
134
135 static ToleranceProvider of(final CartesianToleranceProvider cartesianToleranceProvider) {
136 return new ToleranceProvider() {
137
138 @Override
139 public double[][] getTolerances(final Vector3D position, final Vector3D velocity) {
140 return cartesianToleranceProvider.getTolerances(position, velocity);
141 }
142
143 @Override
144 public double[][] getTolerances(final Orbit referenceOrbit, final OrbitType propagationOrbitType,
145 final PositionAngleType positionAngleType) {
146
147 final double[][] cartesianTolerances = getTolerances(referenceOrbit.getPosition(),
148 referenceOrbit.getPVCoordinates().getVelocity());
149 if (propagationOrbitType == OrbitType.CARTESIAN) {
150 return cartesianTolerances;
151 }
152
153 final double[] cartAbsTol = cartesianTolerances[0];
154 final double[] cartRelTol = cartesianTolerances[1];
155 final double[] absTol = new double[7];
156 final double[] relTol = absTol.clone();
157
158
159 final double[][] jacobian = new double[6][6];
160 final Orbit converted = propagationOrbitType.convertType(referenceOrbit);
161 converted.getJacobianWrtCartesian(positionAngleType, jacobian);
162
163 double minimumRel = cartRelTol[6];
164 for (int i = 0; i < jacobian.length; ++i) {
165 final double[] row = jacobian[i];
166 absTol[i] = FastMath.abs(row[0]) * cartAbsTol[0] +
167 FastMath.abs(row[1]) * cartAbsTol[1] +
168 FastMath.abs(row[2]) * cartAbsTol[2] +
169 FastMath.abs(row[3]) * cartAbsTol[3] +
170 FastMath.abs(row[4]) * cartAbsTol[4] +
171 FastMath.abs(row[5]) * cartAbsTol[5];
172 if (Double.isNaN(absTol[i])) {
173 throw new OrekitException(OrekitMessages.SINGULAR_JACOBIAN_FOR_ORBIT_TYPE, propagationOrbitType);
174 }
175 minimumRel = FastMath.min(minimumRel, cartRelTol[i]);
176 }
177 absTol[6] = cartAbsTol[6];
178
179 Arrays.fill(relTol, 0, 6, minimumRel);
180 relTol[6] = cartRelTol[6];
181 return new double[][] {absTol, relTol};
182 }
183 };
184 }
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199 static ToleranceProvider getDefaultToleranceProvider(final double dP) {
200 return new ToleranceProvider() {
201 @Override
202 public double[][] getTolerances(final Orbit referenceOrbit, final OrbitType propagationOrbitType,
203 final PositionAngleType positionAngleType) {
204
205 final double[] relTol = new double[7];
206 final double[] cartAbsTol = new double[7];
207 final double relPos = dP / referenceOrbit.getPosition().getNorm();
208 Arrays.fill(relTol, 0, relTol.length, relPos);
209 Arrays.fill(cartAbsTol, 0, 3, dP);
210
211 final PVCoordinates pv = referenceOrbit.getPVCoordinates();
212 final double r2 = pv.getPosition().getNormSq();
213 final double v = pv.getVelocity().getNorm();
214 final double dV = referenceOrbit.getMu() * dP / (v * r2);
215 Arrays.fill(cartAbsTol, 3, 6, dV);
216 cartAbsTol[6] = DEFAULT_ABSOLUTE_MASS_TOLERANCE;
217
218 if (propagationOrbitType == OrbitType.CARTESIAN) {
219 return new double[][] {cartAbsTol, relTol};
220 }
221
222
223 final double[] absTol = cartAbsTol.clone();
224 final double[][] jacobian = new double[6][6];
225 final Orbit converted = propagationOrbitType.convertType(referenceOrbit);
226 converted.getJacobianWrtCartesian(PositionAngleType.TRUE, jacobian);
227
228 for (int i = 0; i < jacobian.length; ++i) {
229 final double[] row = jacobian[i];
230 absTol[i] = FastMath.abs(row[0]) * cartAbsTol[0] +
231 FastMath.abs(row[1]) * cartAbsTol[1] +
232 FastMath.abs(row[2]) * cartAbsTol[2] +
233 FastMath.abs(row[3]) * cartAbsTol[3] +
234 FastMath.abs(row[4]) * cartAbsTol[4] +
235 FastMath.abs(row[5]) * cartAbsTol[5];
236 if (Double.isNaN(absTol[i])) {
237 throw new OrekitException(OrekitMessages.SINGULAR_JACOBIAN_FOR_ORBIT_TYPE, propagationOrbitType);
238 }
239 }
240
241 return new double[][] {absTol, relTol};
242 }
243
244 @Override
245 public double[][] getTolerances(final Vector3D position, final Vector3D velocity) {
246 final double[] absTol = new double[7];
247 final double[] relTol = absTol.clone();
248 final double relPos = dP / position.getNorm();
249 Arrays.fill(relTol, 0, relTol.length, relPos);
250 Arrays.fill(absTol, 0, 3, dP);
251 final double dV = relPos * velocity.getNorm();
252 Arrays.fill(absTol, 3, 6, dV);
253 absTol[6] = DEFAULT_ABSOLUTE_MASS_TOLERANCE;
254 return new double[][] {absTol, relTol};
255 }
256 };
257 }
258 }