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