1 /* Copyright 2002-2026 CS GROUP
2 * Licensed to CS GROUP (CS) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * CS licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17 package org.orekit.propagation.conversion;
18
19 import java.util.ArrayList;
20 import java.util.Arrays;
21 import java.util.List;
22
23 import org.hipparchus.analysis.MultivariateVectorFunction;
24 import org.hipparchus.exception.MathRuntimeException;
25 import org.hipparchus.linear.DiagonalMatrix;
26 import org.hipparchus.optim.ConvergenceChecker;
27 import org.hipparchus.optim.SimpleVectorValueChecker;
28 import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresBuilder;
29 import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresFactory;
30 import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer;
31 import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem;
32 import org.hipparchus.optim.nonlinear.vector.leastsquares.LevenbergMarquardtOptimizer;
33 import org.hipparchus.optim.nonlinear.vector.leastsquares.MultivariateJacobianFunction;
34 import org.hipparchus.util.FastMath;
35 import org.orekit.errors.OrekitException;
36 import org.orekit.errors.OrekitMessages;
37 import org.orekit.frames.Frame;
38 import org.orekit.propagation.Propagator;
39 import org.orekit.propagation.SpacecraftState;
40 import org.orekit.propagation.integration.AbstractIntegratedPropagator;
41 import org.orekit.time.AbsoluteDate;
42 import org.orekit.utils.PVCoordinates;
43 import org.orekit.utils.ParameterDriver;
44
45 /** Common handling of {@link PropagatorConverter} methods for propagators conversions.
46 * <p>
47 * This abstract class factors the common code for propagators conversion.
48 * Only one method must be implemented by derived classes: {@link #getObjectiveFunction()}.
49 * </p>
50 * <p>
51 * The converter uses the LevenbergMarquardtOptimizer from the <a
52 * href="https://hipparchus.org/">Hipparchus</a> library.
53 * Different implementations correspond to different methods for computing the Jacobian.
54 * </p>
55 * @author Pascal Parraud
56 * @since 6.0
57 */
58 public abstract class AbstractPropagatorConverter implements PropagatorConverter {
59
60 /** Spacecraft states sample. */
61 private List<SpacecraftState> sample;
62
63 /** Target position and velocities at sample points. */
64 private double[] target;
65
66 /** Weight for residuals. */
67 private double[] weight;
68
69 /** Position use indicator. */
70 private boolean onlyPosition;
71
72 /** Adapted propagator. */
73 private Propagator adapted;
74
75 /** Propagator builder. */
76 private final PropagatorBuilder builder;
77
78 /** Frame. */
79 private final Frame frame;
80
81 /** Optimizer for fitting. */
82 private final LevenbergMarquardtOptimizer optimizer;
83
84 /** Optimum found. */
85 private LeastSquaresOptimizer.Optimum optimum;
86
87 /** Convergence checker for optimization algorithm. */
88 private final ConvergenceChecker<LeastSquaresProblem.Evaluation> checker;
89
90 /** Maximum number of iterations for optimization. */
91 private final int maxIterations;
92
93 /** Build a new instance.
94 * @param builder propagator builder
95 * @param threshold absolute convergence threshold for optimization algorithm
96 * @param maxIterations maximum number of iterations for fitting
97 */
98 protected AbstractPropagatorConverter(final PropagatorBuilder builder,
99 final double threshold,
100 final int maxIterations) {
101 this.builder = builder;
102 this.frame = builder.getFrame();
103 this.optimizer = new LevenbergMarquardtOptimizer();
104 this.maxIterations = maxIterations;
105 this.sample = new ArrayList<>();
106
107 final SimpleVectorValueChecker svvc = new SimpleVectorValueChecker(-1.0, threshold);
108 this.checker = LeastSquaresFactory.evaluationChecker(svvc);
109
110 }
111
112 /** Convert a propagator to another.
113 * @param source initial propagator (the propagator will be used for sample
114 * generation, if it is a numerical propagator, its initial state will
115 * be reset unless {@link AbstractIntegratedPropagator#setResetAtEnd(boolean)}
116 * has been called beforehand)
117 * @param timeSpan time span for fitting
118 * @param nbPoints number of fitting points over time span
119 * @param freeParameters names of the free parameters
120 * @return adapted propagator
121 * @exception IllegalArgumentException if one of the parameters cannot be free
122 */
123 public Propagator convert(final Propagator source,
124 final double timeSpan,
125 final int nbPoints,
126 final List<String> freeParameters)
127 throws IllegalArgumentException {
128 setFreeParameters(freeParameters);
129 final List<SpacecraftState> states = createSample(source, timeSpan, nbPoints);
130 return convert(states, false, freeParameters);
131 }
132
133 /** Convert a propagator to another.
134 * @param source initial propagator (the propagator will be used for sample
135 * generation, if it is a numerical propagator, its initial state will
136 * be reset unless {@link AbstractIntegratedPropagator#setResetAtEnd(boolean)}
137 * has been called beforehand)
138 * @param timeSpan time span for fitting
139 * @param nbPoints number of fitting points over time span
140 * @param freeParameters names of the free parameters
141 * @return adapted propagator
142 * @exception IllegalArgumentException if one of the parameters cannot be free
143 */
144 public Propagator convert(final Propagator source,
145 final double timeSpan,
146 final int nbPoints,
147 final String... freeParameters)
148 throws IllegalArgumentException {
149 setFreeParameters(Arrays.asList(freeParameters));
150 final List<SpacecraftState> states = createSample(source, timeSpan, nbPoints);
151 return convert(states, false, freeParameters);
152 }
153
154 /** Find the propagator that minimize the mean square error for a sample of {@link SpacecraftState states}.
155 * @param states spacecraft states sample to fit
156 * @param positionOnly if true, consider only position data otherwise both position and velocity are used
157 * @param freeParameters names of the free parameters
158 * @return adapted propagator
159 * @exception IllegalArgumentException if one of the parameters cannot be free
160 */
161 public Propagator convert(final List<SpacecraftState> states,
162 final boolean positionOnly,
163 final List<String> freeParameters)
164 throws IllegalArgumentException {
165 setFreeParameters(freeParameters);
166 return adapt(states, positionOnly);
167 }
168
169 /** Find the propagator that minimize the mean square error for a sample of {@link SpacecraftState states}.
170 * @param states spacecraft states sample to fit
171 * @param positionOnly if true, consider only position data otherwise both position and velocity are used
172 * @param freeParameters names of the free parameters
173 * @return adapted propagator
174 * @exception IllegalArgumentException if one of the parameters cannot be free
175 */
176 public Propagator convert(final List<SpacecraftState> states,
177 final boolean positionOnly,
178 final String... freeParameters)
179 throws IllegalArgumentException {
180 setFreeParameters(Arrays.asList(freeParameters));
181 return adapt(states, positionOnly);
182 }
183
184 /** Get the adapted propagator.
185 * @return adapted propagator
186 */
187 public Propagator getAdaptedPropagator() {
188 return adapted;
189 }
190
191 /** Get the Root Mean Square Deviation of the fitting.
192 * @return RMSD
193 */
194 public double getRMS() {
195 return optimum.getRMS();
196 }
197
198 /** Get the number of objective function evaluations.
199 * @return the number of objective function evaluations.
200 */
201 public int getEvaluations() {
202 return optimum.getEvaluations();
203 }
204
205 /** Get the function computing position/velocity at sample points.
206 * @return function computing position/velocity at sample points
207 */
208 protected abstract MultivariateVectorFunction getObjectiveFunction();
209
210 /** Get the Jacobian of the function computing position/velocity at sample points.
211 * @return Jacobian of the function computing position/velocity at sample points
212 */
213 protected abstract MultivariateJacobianFunction getModel();
214
215 /** Check if fitting uses only sample positions.
216 * @return true if fitting uses only sample positions
217 */
218 protected boolean isOnlyPosition() {
219 return onlyPosition;
220 }
221
222 /** Get the size of the target.
223 * @return target size
224 */
225 protected int getTargetSize() {
226 return target.length;
227 }
228
229 /** Get the frame of the initial state.
230 * @return the orbit frame
231 */
232 protected Frame getFrame() {
233 return frame;
234 }
235
236 /** Get the states sample.
237 * @return the states sample
238 */
239 protected List<SpacecraftState> getSample() {
240 return sample;
241 }
242
243 /** Create a sample of {@link SpacecraftState}.
244 * @param source initial propagator
245 * @param timeSpan time span for the sample
246 * @param nbPoints number of points for the sample over the time span
247 * @return a sample of {@link SpacecraftState}
248 */
249 private List<SpacecraftState> createSample(final Propagator source,
250 final double timeSpan,
251 final int nbPoints) {
252
253 final List<SpacecraftState> states = new ArrayList<>();
254
255 final double stepSize = timeSpan / (nbPoints - 1);
256 final AbsoluteDate iniDate = source.getInitialState().getDate();
257 for (double dt = 0; dt < timeSpan; dt += stepSize) {
258 states.add(source.propagate(iniDate.shiftedBy(dt)));
259 }
260
261 return states;
262 }
263
264 /** Free some parameters.
265 * @param freeParameters names of the free parameters
266 */
267 private void setFreeParameters(final Iterable<String> freeParameters) {
268
269 // start by setting all parameters as not estimated
270 for (final ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {
271 driver.setSelected(false);
272 }
273
274 // set only the selected parameters as estimated
275 for (final String parameter : freeParameters) {
276 boolean found = false;
277 for (final ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {
278 if (driver.getName().equals(parameter)) {
279 found = true;
280 driver.setSelected(true);
281 break;
282 }
283 }
284 if (!found) {
285 // build the list of supported parameters
286 final StringBuilder sBuilder = new StringBuilder();
287 for (final ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {
288 if (sBuilder.length() > 0) {
289 sBuilder.append(", ");
290 }
291 sBuilder.append(driver.getName());
292 }
293 throw new OrekitException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME,
294 parameter, sBuilder.toString());
295 }
296 }
297 }
298
299 /** Adapt a propagator to minimize the mean square error for a set of {@link SpacecraftState states}.
300 * @param states set of spacecraft states to fit
301 * @param positionOnly if true, consider only position data otherwise both position and velocity are used
302 * @return adapted propagator
303 */
304 private Propagator adapt(final List<SpacecraftState> states,
305 final boolean positionOnly) {
306
307 this.onlyPosition = positionOnly;
308
309 // very rough first guess using osculating parameters of first sample point
310 final double[] initial = builder.getSelectedNormalizedParameters();
311
312 // warm-up iterations, using only a few points
313 final int dataPerPoint = onlyPosition ? 3 : 6;
314 final int neededPoints = FastMath.max(2, (initial.length + dataPerPoint - 1) / dataPerPoint);
315 final int subSampling = (states.size() - 1) / (neededPoints - 1);
316 final List<SpacecraftState> subSampledStates = new ArrayList<>(neededPoints);
317 for (int i = 0; i < neededPoints; i++) {
318 subSampledStates.add(states.get(subSampling * i));
319 }
320 setSample(subSampledStates);
321 final double[] intermediate = fit(initial);
322
323 // final search using all points
324 setSample(states);
325 final double[] result = fit(intermediate);
326
327 adapted = buildAdaptedPropagator(result);
328
329 return adapted;
330 }
331
332 /** Find the propagator that minimize the mean square error for a sample of {@link SpacecraftState states}.
333 * @param initial initial estimation parameters (position, velocity, free parameters)
334 * @return fitted parameters
335 * @exception MathRuntimeException if maximal number of iterations is exceeded
336 */
337 private double[] fit(final double[] initial)
338 throws MathRuntimeException {
339
340 final LeastSquaresProblem problem = new LeastSquaresBuilder().
341 maxIterations(maxIterations).
342 maxEvaluations(Integer.MAX_VALUE).
343 model(getModel()).
344 target(target).
345 weight(new DiagonalMatrix(weight)).
346 start(initial).
347 checker(checker).
348 build();
349
350 optimum = optimizer.optimize(problem);
351 return optimum.getPoint().toArray();
352
353 }
354
355 /** Build the adapted propagator for a given position/velocity(/free) parameters set.
356 * @param parameterSet position/velocity(/free) parameters set
357 * @return adapted propagator
358 */
359 private Propagator buildAdaptedPropagator(final double[] parameterSet) {
360 return builder.buildPropagator(parameterSet);
361 }
362
363 /** Set the states sample.
364 * @param states spacecraft states sample
365 */
366 private void setSample(final List<SpacecraftState> states) {
367
368 this.sample = states;
369
370 if (onlyPosition) {
371 target = new double[states.size() * 3];
372 weight = new double[states.size() * 3];
373 } else {
374 target = new double[states.size() * 6];
375 weight = new double[states.size() * 6];
376 }
377
378 int k = 0;
379 for (SpacecraftState state : states) {
380
381 final PVCoordinates pv = state.getPVCoordinates(frame);
382
383 // position
384 target[k] = pv.getPosition().getX();
385 weight[k++] = 1;
386 target[k] = pv.getPosition().getY();
387 weight[k++] = 1;
388 target[k] = pv.getPosition().getZ();
389 weight[k++] = 1;
390
391 // velocity
392 if (!onlyPosition) {
393 // velocity weight relative to position
394 final double r2 = pv.getPosition().getNorm2Sq();
395 final double v = pv.getVelocity().getNorm();
396 final double vWeight = v * r2 / state.getOrbit().getMu();
397
398 target[k] = pv.getVelocity().getX();
399 weight[k++] = vWeight;
400 target[k] = pv.getVelocity().getY();
401 weight[k++] = vWeight;
402 target[k] = pv.getVelocity().getZ();
403 weight[k++] = vWeight;
404 }
405
406 }
407
408 }
409
410 }