1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.propagation.conversion;
18
19 import java.util.List;
20
21 import org.hipparchus.analysis.MultivariateVectorFunction;
22 import org.hipparchus.linear.ArrayRealVector;
23 import org.hipparchus.linear.MatrixUtils;
24 import org.hipparchus.linear.RealMatrix;
25 import org.hipparchus.linear.RealVector;
26 import org.hipparchus.optim.nonlinear.vector.leastsquares.MultivariateJacobianFunction;
27 import org.hipparchus.util.Pair;
28 import org.orekit.errors.OrekitException;
29 import org.orekit.errors.OrekitMessages;
30 import org.orekit.orbits.OrbitType;
31 import org.orekit.propagation.MatricesHarvester;
32 import org.orekit.propagation.SpacecraftState;
33 import org.orekit.propagation.numerical.NumericalPropagator;
34 import org.orekit.propagation.sampling.OrekitStepHandler;
35 import org.orekit.propagation.sampling.OrekitStepInterpolator;
36 import org.orekit.time.AbsoluteDate;
37 import org.orekit.utils.PVCoordinates;
38 import org.orekit.utils.ParameterDriver;
39 import org.orekit.utils.ParameterDriversList;
40 import org.orekit.utils.TimeSpanMap.Span;
41
42
43
44
45
46 public class JacobianPropagatorConverter extends AbstractPropagatorConverter {
47
48
49 private final NumericalPropagatorBuilder builder;
50
51
52
53
54
55
56
57 public JacobianPropagatorConverter(final NumericalPropagatorBuilder builder,
58 final double threshold,
59 final int maxIterations) {
60 super(builder, threshold, maxIterations);
61 if (builder.getOrbitType() != OrbitType.CARTESIAN) {
62 throw new OrekitException(OrekitMessages.ORBIT_TYPE_NOT_ALLOWED,
63 builder.getOrbitType(), OrbitType.CARTESIAN);
64 }
65 this.builder = builder;
66 }
67
68
69 protected MultivariateVectorFunction getObjectiveFunction() {
70 return point -> {
71 final NumericalPropagator propagator = builder.buildPropagator(point);
72 final ValuesHandler handler = new ValuesHandler();
73 propagator.getMultiplexer().add(handler);
74 final List<SpacecraftState> sample = getSample();
75 propagator.propagate(sample.get(sample.size() - 1).getDate().shiftedBy(10.0));
76 return handler.value;
77 };
78 }
79
80
81 protected MultivariateJacobianFunction getModel() {
82 return point -> {
83 final NumericalPropagator propagator = builder.buildPropagator(point.toArray());
84 final JacobianHandler handler = new JacobianHandler(propagator, point.getDimension());
85 propagator.getMultiplexer().add(handler);
86 final List<SpacecraftState> sample = getSample();
87 propagator.propagate(sample.get(sample.size() - 1).getDate().shiftedBy(10.0));
88 return new Pair<>(handler.value, handler.jacobian);
89 };
90 }
91
92
93
94
95
96
97
98 private class ValuesHandler implements OrekitStepHandler {
99
100
101 private final double[] value;
102
103
104 private int number;
105
106
107 private int index;
108
109
110
111 ValuesHandler() {
112 this.value = new double[getTargetSize()];
113 }
114
115
116 @Override
117 public void init(final SpacecraftState initialState, final AbsoluteDate target) {
118 number = 0;
119 index = 0;
120 }
121
122
123 @Override
124 public void handleStep(final OrekitStepInterpolator interpolator) {
125
126 while (number < getSample().size()) {
127
128
129 final SpacecraftState next = getSample().get(number);
130
131
132 final AbsoluteDate currentDate = interpolator.getCurrentState().getDate();
133 if (next.getDate().compareTo(currentDate) > 0) {
134 return;
135 }
136
137 final PVCoordinates pv = interpolator.getInterpolatedState(next.getDate()).getPVCoordinates(getFrame());
138 value[index++] = pv.getPosition().getX();
139 value[index++] = pv.getPosition().getY();
140 value[index++] = pv.getPosition().getZ();
141 if (!isOnlyPosition()) {
142 value[index++] = pv.getVelocity().getX();
143 value[index++] = pv.getVelocity().getY();
144 value[index++] = pv.getVelocity().getZ();
145 }
146
147
148 ++number;
149
150 }
151
152 }
153
154 }
155
156
157
158
159
160
161
162 private class JacobianHandler implements OrekitStepHandler {
163
164
165 private final RealVector value;
166
167
168 private final RealMatrix jacobian;
169
170
171 private final int stateSize;
172
173
174 private final MatricesHarvester harvester;
175
176
177 private int number;
178
179
180 private int index;
181
182
183
184
185
186 JacobianHandler(final NumericalPropagator propagator, final int columns) {
187 this.value = new ArrayRealVector(getTargetSize());
188 this.jacobian = MatrixUtils.createRealMatrix(getTargetSize(), columns);
189 this.stateSize = isOnlyPosition() ? 3 : 6;
190 this.harvester = propagator.setupMatricesComputation("converter-partials", null, null);
191 }
192
193
194 @Override
195 public void init(final SpacecraftState initialState, final AbsoluteDate target) {
196 number = 0;
197 index = 0;
198 }
199
200
201 @Override
202 public void handleStep(final OrekitStepInterpolator interpolator) {
203
204 while (number < getSample().size()) {
205
206
207 final SpacecraftState next = getSample().get(number);
208
209
210 final AbsoluteDate currentDate = interpolator.getCurrentState().getDate();
211 if (next.getDate().compareTo(currentDate) > 0) {
212 return;
213 }
214
215 fillRows(index, interpolator.getInterpolatedState(next.getDate()),
216 builder.getOrbitalParametersDrivers());
217
218
219 ++number;
220 index += stateSize;
221
222 }
223
224 }
225
226
227
228
229
230
231 private void fillRows(final int row,
232 final SpacecraftState state,
233 final ParameterDriversList orbitalParameters) {
234
235
236 final PVCoordinates pv = state.getPVCoordinates(getFrame());
237 value.setEntry(row, pv.getPosition().getX());
238 value.setEntry(row + 1, pv.getPosition().getY());
239 value.setEntry(row + 2, pv.getPosition().getZ());
240 if (!isOnlyPosition()) {
241 value.setEntry(row + 3, pv.getVelocity().getX());
242 value.setEntry(row + 4, pv.getVelocity().getY());
243 value.setEntry(row + 5, pv.getVelocity().getZ());
244 }
245
246
247 final RealMatrix dYdY0 = harvester.getStateTransitionMatrix(state);
248 final RealMatrix dYdP = harvester.getParametersJacobian(state);
249 for (int k = 0; k < stateSize; k++) {
250 int column = 0;
251 for (int j = 0; j < orbitalParameters.getNbParams(); ++j) {
252 final ParameterDriver driver = orbitalParameters.getDrivers().get(j);
253 if (driver.isSelected()) {
254 jacobian.setEntry(row + k, column++, dYdY0.getEntry(k, j) * driver.getScale());
255 }
256 }
257 if (dYdP != null) {
258 for (int j = 0; j < dYdP.getColumnDimension(); ++j) {
259 final String name = harvester.getJacobiansColumnsNames().get(j);
260 for (final ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {
261
262 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
263 if (name.equals(span.getData())) {
264 jacobian.setEntry(row + k, column++, dYdP.getEntry(k, j) * driver.getScale());
265 }
266 }
267 }
268 }
269 }
270 }
271 }
272
273 }
274
275 }
276