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