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.ode.events.Action;
27 import org.hipparchus.optim.nonlinear.vector.leastsquares.MultivariateJacobianFunction;
28 import org.hipparchus.util.Pair;
29 import org.orekit.errors.OrekitException;
30 import org.orekit.errors.OrekitMessages;
31 import org.orekit.orbits.OrbitType;
32 import org.orekit.propagation.SpacecraftState;
33 import org.orekit.propagation.events.DateDetector;
34 import org.orekit.propagation.events.handlers.EventHandler;
35 import org.orekit.propagation.numerical.JacobiansMapper;
36 import org.orekit.propagation.numerical.NumericalPropagator;
37 import org.orekit.propagation.numerical.PartialDerivativesEquations;
38 import org.orekit.utils.PVCoordinates;
39 import org.orekit.utils.ParameterDriver;
40 import org.orekit.utils.ParameterDriversList;
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 new MultivariateVectorFunction() {
71
72
73 public double[] value(final double[] arg)
74 throws IllegalArgumentException, OrekitException {
75
76 final double[] value = new double[getTargetSize()];
77
78 final NumericalPropagator prop = builder.buildPropagator(arg);
79
80 final int stateSize = isOnlyPosition() ? 3 : 6;
81 final List<SpacecraftState> sample = getSample();
82 for (int i = 0; i < sample.size(); ++i) {
83 final int row = i * stateSize;
84 if (prop.getInitialState().getDate().equals(sample.get(i).getDate())) {
85
86 fillRows(value, row, prop.getInitialState());
87 } else {
88
89 prop.addEventDetector(new DateDetector(sample.get(i).getDate()).withHandler(new EventHandler<DateDetector>() {
90
91 @Override
92 public Action eventOccurred(final SpacecraftState state, final DateDetector detector,
93 final boolean increasing) {
94 fillRows(value, row, state);
95 return row + stateSize >= getTargetSize() ? Action.STOP : Action.CONTINUE;
96 }
97 }));
98 }
99 }
100
101 prop.propagate(sample.get(sample.size() - 1).getDate().shiftedBy(10.0));
102
103 return value;
104 }
105 };
106 }
107
108
109 protected MultivariateJacobianFunction getModel() {
110 return new MultivariateJacobianFunction() {
111
112
113 public Pair<RealVector, RealMatrix> value(final RealVector point)
114 throws IllegalArgumentException, OrekitException {
115
116 final RealVector value = new ArrayRealVector(getTargetSize());
117 final RealMatrix jacobian = MatrixUtils.createRealMatrix(getTargetSize(), point.getDimension());
118
119 final NumericalPropagator prop = builder.buildPropagator(point.toArray());
120 final int stateSize = isOnlyPosition() ? 3 : 6;
121 final ParameterDriversList orbitalParameters = builder.getOrbitalParametersDrivers();
122 final PartialDerivativesEquations pde = new PartialDerivativesEquations("pde", prop);
123 final ParameterDriversList propagationParameters = pde.getSelectedParameters();
124 prop.setInitialState(pde.setInitialJacobians(prop.getInitialState()));
125 final JacobiansMapper mapper = pde.getMapper();
126
127 final List<SpacecraftState> sample = getSample();
128 for (int i = 0; i < sample.size(); ++i) {
129 final int row = i * stateSize;
130 if (prop.getInitialState().getDate().equals(sample.get(i).getDate())) {
131
132 fillRows(value, jacobian, row, prop.getInitialState(), stateSize,
133 orbitalParameters, propagationParameters, mapper);
134 } else {
135
136 prop.addEventDetector(new DateDetector(sample.get(i).getDate()).withHandler(new EventHandler<DateDetector>() {
137
138 @Override
139 public Action eventOccurred(final SpacecraftState state, final DateDetector detector,
140 final boolean increasing) {
141 fillRows(value, jacobian, row, state, stateSize,
142 orbitalParameters, propagationParameters, mapper);
143 return row + stateSize >= getTargetSize() ? Action.STOP : Action.CONTINUE;
144 }
145 }));
146 }
147 }
148
149 prop.propagate(sample.get(sample.size() - 1).getDate().shiftedBy(10.0));
150
151 return new Pair<RealVector, RealMatrix>(value, jacobian);
152 }
153 };
154 }
155
156
157
158
159
160
161 private void fillRows(final double[] value, final int row, final SpacecraftState state) {
162 final PVCoordinates pv = state.getPVCoordinates(getFrame());
163 value[row ] = pv.getPosition().getX();
164 value[row + 1] = pv.getPosition().getY();
165 value[row + 2] = pv.getPosition().getZ();
166 if (!isOnlyPosition()) {
167 value[row + 3] = pv.getVelocity().getX();
168 value[row + 4] = pv.getVelocity().getY();
169 value[row + 5] = pv.getVelocity().getZ();
170 }
171 }
172
173
174
175
176
177
178
179
180
181
182
183 private void fillRows(final RealVector value, final RealMatrix jacobian, final int row,
184 final SpacecraftState state, final int stateSize,
185 final ParameterDriversList orbitalParameters,
186 final ParameterDriversList propagationParameters,
187 final JacobiansMapper mapper) {
188
189
190 final PVCoordinates pv = state.getPVCoordinates(getFrame());
191 value.setEntry(row, pv.getPosition().getX());
192 value.setEntry(row + 1, pv.getPosition().getY());
193 value.setEntry(row + 2, pv.getPosition().getZ());
194 if (!isOnlyPosition()) {
195 value.setEntry(row + 3, pv.getVelocity().getX());
196 value.setEntry(row + 4, pv.getVelocity().getY());
197 value.setEntry(row + 5, pv.getVelocity().getZ());
198 }
199
200
201 final double[][] dYdY0 = new double[JacobiansMapper.STATE_DIMENSION][JacobiansMapper.STATE_DIMENSION];
202 final double[][] dYdP = new double[JacobiansMapper.STATE_DIMENSION][mapper.getParameters()];
203 mapper.getStateJacobian(state, dYdY0);
204 mapper.getParametersJacobian(state, dYdP);
205 for (int k = 0; k < stateSize; k++) {
206 int index = 0;
207 for (int j = 0; j < orbitalParameters.getNbParams(); ++j) {
208 final ParameterDriver driver = orbitalParameters.getDrivers().get(j);
209 if (driver.isSelected()) {
210 jacobian.setEntry(row + k, index++, dYdY0[k][j] * driver.getScale());
211 }
212 }
213 for (int j = 0; j < propagationParameters.getNbParams(); ++j) {
214 final ParameterDriver driver = propagationParameters.getDrivers().get(j);
215 jacobian.setEntry(row + k, index++, dYdP[k][j] * driver.getScale());
216 }
217 }
218
219 }
220
221 }
222