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