1 /* Copyright 2002-2025 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 /** Auxiliary outputData: RMS of solution. */
70 private double rms;
71
72 /** Position use indicator. */
73 private boolean onlyPosition;
74
75 /** Adapted propagator. */
76 private Propagator adapted;
77
78 /** Propagator builder. */
79 private final PropagatorBuilder builder;
80
81 /** Frame. */
82 private final Frame frame;
83
84 /** Optimizer for fitting. */
85 private final LevenbergMarquardtOptimizer optimizer;
86
87 /** Optimum found. */
88 private LeastSquaresOptimizer.Optimum optimum;
89
90 /** Convergence checker for optimization algorithm. */
91 private final ConvergenceChecker<LeastSquaresProblem.Evaluation> checker;
92
93 /** Maximum number of iterations for optimization. */
94 private final int maxIterations;
95
96 /** Build a new instance.
97 * @param builder propagator builder
98 * @param threshold absolute convergence threshold for optimization algorithm
99 * @param maxIterations maximum number of iterations for fitting
100 */
101 protected AbstractPropagatorConverter(final PropagatorBuilder builder,
102 final double threshold,
103 final int maxIterations) {
104 this.builder = builder;
105 this.frame = builder.getFrame();
106 this.optimizer = new LevenbergMarquardtOptimizer();
107 this.maxIterations = maxIterations;
108 this.sample = new ArrayList<>();
109
110 final SimpleVectorValueChecker svvc = new SimpleVectorValueChecker(-1.0, threshold);
111 this.checker = LeastSquaresFactory.evaluationChecker(svvc);
112
113 }
114
115 /** Convert a propagator to another.
116 * @param source initial propagator (the propagator will be used for sample
117 * generation, if it is a numerical propagator, its initial state will
118 * be reset unless {@link AbstractIntegratedPropagator#setResetAtEnd(boolean)}
119 * has been called beforehand)
120 * @param timeSpan time span for fitting
121 * @param nbPoints number of fitting points over time span
122 * @param freeParameters names of the free parameters
123 * @return adapted propagator
124 * @exception IllegalArgumentException if one of the parameters cannot be free
125 */
126 public Propagator convert(final Propagator source,
127 final double timeSpan,
128 final int nbPoints,
129 final List<String> freeParameters)
130 throws IllegalArgumentException {
131 setFreeParameters(freeParameters);
132 final List<SpacecraftState> states = createSample(source, timeSpan, nbPoints);
133 return convert(states, false, freeParameters);
134 }
135
136 /** Convert a propagator to another.
137 * @param source initial propagator (the propagator will be used for sample
138 * generation, if it is a numerical propagator, its initial state will
139 * be reset unless {@link AbstractIntegratedPropagator#setResetAtEnd(boolean)}
140 * has been called beforehand)
141 * @param timeSpan time span for fitting
142 * @param nbPoints number of fitting points over time span
143 * @param freeParameters names of the free parameters
144 * @return adapted propagator
145 * @exception IllegalArgumentException if one of the parameters cannot be free
146 */
147 public Propagator convert(final Propagator source,
148 final double timeSpan,
149 final int nbPoints,
150 final String... freeParameters)
151 throws IllegalArgumentException {
152 setFreeParameters(Arrays.asList(freeParameters));
153 final List<SpacecraftState> states = createSample(source, timeSpan, nbPoints);
154 return convert(states, false, freeParameters);
155 }
156
157 /** Find the propagator that minimize the mean square error for a sample of {@link SpacecraftState states}.
158 * @param states spacecraft states sample to fit
159 * @param positionOnly if true, consider only position data otherwise both position and velocity are used
160 * @param freeParameters names of the free parameters
161 * @return adapted propagator
162 * @exception IllegalArgumentException if one of the parameters cannot be free
163 */
164 public Propagator convert(final List<SpacecraftState> states,
165 final boolean positionOnly,
166 final List<String> freeParameters)
167 throws IllegalArgumentException {
168 setFreeParameters(freeParameters);
169 return adapt(states, positionOnly);
170 }
171
172 /** Find the propagator that minimize the mean square error for a sample of {@link SpacecraftState states}.
173 * @param states spacecraft states sample to fit
174 * @param positionOnly if true, consider only position data otherwise both position and velocity are used
175 * @param freeParameters names of the free parameters
176 * @return adapted propagator
177 * @exception IllegalArgumentException if one of the parameters cannot be free
178 */
179 public Propagator convert(final List<SpacecraftState> states,
180 final boolean positionOnly,
181 final String... freeParameters)
182 throws IllegalArgumentException {
183 setFreeParameters(Arrays.asList(freeParameters));
184 return adapt(states, positionOnly);
185 }
186
187 /** Get the adapted propagator.
188 * @return adapted propagator
189 */
190 public Propagator getAdaptedPropagator() {
191 return adapted;
192 }
193
194 /** Get the Root Mean Square Deviation of the fitting.
195 * @return RMSD
196 */
197 public double getRMS() {
198 return rms;
199 }
200
201 /** Get the number of objective function evaluations.
202 * @return the number of objective function evaluations.
203 */
204 public int getEvaluations() {
205 return optimum.getEvaluations();
206 }
207
208 /** Get the function computing position/velocity at sample points.
209 * @return function computing position/velocity at sample points
210 */
211 protected abstract MultivariateVectorFunction getObjectiveFunction();
212
213 /** Get the Jacobian of the function computing position/velocity at sample points.
214 * @return Jacobian of the function computing position/velocity at sample points
215 */
216 protected abstract MultivariateJacobianFunction getModel();
217
218 /** Check if fitting uses only sample positions.
219 * @return true if fitting uses only sample positions
220 */
221 protected boolean isOnlyPosition() {
222 return onlyPosition;
223 }
224
225 /** Get the size of the target.
226 * @return target size
227 */
228 protected int getTargetSize() {
229 return target.length;
230 }
231
232 /** Get the frame of the initial state.
233 * @return the orbit frame
234 */
235 protected Frame getFrame() {
236 return frame;
237 }
238
239 /** Get the states sample.
240 * @return the states sample
241 */
242 protected List<SpacecraftState> getSample() {
243 return sample;
244 }
245
246 /** Create a sample of {@link SpacecraftState}.
247 * @param source initial propagator
248 * @param timeSpan time span for the sample
249 * @param nbPoints number of points for the sample over the time span
250 * @return a sample of {@link SpacecraftState}
251 */
252 private List<SpacecraftState> createSample(final Propagator source,
253 final double timeSpan,
254 final int nbPoints) {
255
256 final List<SpacecraftState> states = new ArrayList<>();
257
258 final double stepSize = timeSpan / (nbPoints - 1);
259 final AbsoluteDate iniDate = source.getInitialState().getDate();
260 for (double dt = 0; dt < timeSpan; dt += stepSize) {
261 states.add(source.propagate(iniDate.shiftedBy(dt)));
262 }
263
264 return states;
265 }
266
267 /** Free some parameters.
268 * @param freeParameters names of the free parameters
269 */
270 private void setFreeParameters(final Iterable<String> freeParameters) {
271
272 // start by setting all parameters as not estimated
273 for (final ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {
274 driver.setSelected(false);
275 }
276
277 // set only the selected parameters as estimated
278 for (final String parameter : freeParameters) {
279 boolean found = false;
280 for (final ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {
281 if (driver.getName().equals(parameter)) {
282 found = true;
283 driver.setSelected(true);
284 break;
285 }
286 }
287 if (!found) {
288 // build the list of supported parameters
289 final StringBuilder sBuilder = new StringBuilder();
290 for (final ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {
291 if (sBuilder.length() > 0) {
292 sBuilder.append(", ");
293 }
294 sBuilder.append(driver.getName());
295 }
296 throw new OrekitException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME,
297 parameter, sBuilder.toString());
298 }
299 }
300 }
301
302 /** Adapt a propagator to minimize the mean square error for a set of {@link SpacecraftState states}.
303 * @param states set of spacecraft states to fit
304 * @param positionOnly if true, consider only position data otherwise both position and velocity are used
305 * @return adapted propagator
306 */
307 private Propagator adapt(final List<SpacecraftState> states,
308 final boolean positionOnly) {
309
310 this.onlyPosition = positionOnly;
311
312 // very rough first guess using osculating parameters of first sample point
313 final double[] initial = builder.getSelectedNormalizedParameters();
314
315 // warm-up iterations, using only a few points
316 setSample(states.subList(0, onlyPosition ? 2 : 1));
317 final double[] intermediate = fit(initial);
318
319 // final search using all points
320 setSample(states);
321 final double[] result = fit(intermediate);
322
323 rms = getRMS(result);
324 adapted = buildAdaptedPropagator(result);
325
326 return adapted;
327 }
328
329 /** Find the propagator that minimize the mean square error for a sample of {@link SpacecraftState states}.
330 * @param initial initial estimation parameters (position, velocity, free parameters)
331 * @return fitted parameters
332 * @exception MathRuntimeException if maximal number of iterations is exceeded
333 */
334 private double[] fit(final double[] initial)
335 throws MathRuntimeException {
336
337 final LeastSquaresProblem problem = new LeastSquaresBuilder().
338 maxIterations(maxIterations).
339 maxEvaluations(Integer.MAX_VALUE).
340 model(getModel()).
341 target(target).
342 weight(new DiagonalMatrix(weight)).
343 start(initial).
344 checker(checker).
345 build();
346
347 optimum = optimizer.optimize(problem);
348 return optimum.getPoint().toArray();
349
350 }
351
352 /** Get the Root Mean Square Deviation for a given parameters set.
353 * @param parameterSet position/velocity parameters set
354 * @return RMSD
355 */
356 private double getRMS(final double[] parameterSet) {
357 final double[] residuals = getObjectiveFunction().value(parameterSet);
358 for (int i = 0; i < residuals.length; ++i) {
359 residuals[i] = target[i] - residuals[i];
360 }
361 double sum2 = 0;
362 for (final double residual : residuals) {
363 sum2 += residual * residual;
364 }
365 return FastMath.sqrt(sum2 / residuals.length);
366 }
367
368 /** Build the adpated propagator for a given position/velocity(/free) parameters set.
369 * @param parameterSet position/velocity(/free) parameters set
370 * @return adapted propagator
371 */
372 private Propagator buildAdaptedPropagator(final double[] parameterSet) {
373 return builder.buildPropagator(parameterSet);
374 }
375
376 /** Set the states sample.
377 * @param states spacecraft states sample
378 */
379 private void setSample(final List<SpacecraftState> states) {
380
381 this.sample = states;
382
383 if (onlyPosition) {
384 target = new double[states.size() * 3];
385 weight = new double[states.size() * 3];
386 } else {
387 target = new double[states.size() * 6];
388 weight = new double[states.size() * 6];
389 }
390
391 int k = 0;
392 for (SpacecraftState state : states) {
393
394 final PVCoordinates pv = state.getPVCoordinates(frame);
395
396 // position
397 target[k] = pv.getPosition().getX();
398 weight[k++] = 1;
399 target[k] = pv.getPosition().getY();
400 weight[k++] = 1;
401 target[k] = pv.getPosition().getZ();
402 weight[k++] = 1;
403
404 // velocity
405 if (!onlyPosition) {
406 // velocity weight relative to position
407 final double r2 = pv.getPosition().getNormSq();
408 final double v = pv.getVelocity().getNorm();
409 final double vWeight = v * r2 / state.getOrbit().getMu();
410
411 target[k] = pv.getVelocity().getX();
412 weight[k++] = vWeight;
413 target[k] = pv.getVelocity().getY();
414 weight[k++] = vWeight;
415 target[k] = pv.getVelocity().getZ();
416 weight[k++] = vWeight;
417 }
418
419 }
420
421 }
422
423 }