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.utils;
18
19 import java.util.ArrayList;
20 import java.util.Collection;
21 import java.util.List;
22
23 import org.hipparchus.analysis.ParametricUnivariateFunction;
24 import org.hipparchus.fitting.AbstractCurveFitter;
25 import org.hipparchus.fitting.PolynomialCurveFitter;
26 import org.hipparchus.fitting.WeightedObservedPoint;
27 import org.hipparchus.linear.DiagonalMatrix;
28 import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresBuilder;
29 import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem;
30 import org.hipparchus.util.FastMath;
31 import org.hipparchus.util.SinCos;
32 import org.orekit.time.AbsoluteDate;
33
34 /** Class for fitting evolution of osculating orbital parameters.
35 * <p>
36 * This class allows conversion from osculating parameters to mean parameters.
37 * </p>
38 *
39 * @author Luc Maisonobe
40 */
41 public class SecularAndHarmonic {
42
43 /** Degree of polynomial secular part. */
44 private final int secularDegree;
45
46 /** Pulsations of harmonic part. */
47 private final double[] pulsations;
48
49 /** Reference date for the model. */
50 private AbsoluteDate reference;
51
52 /** Fitted parameters. */
53 private double[] fitted;
54
55 /** Observed points. */
56 private List<WeightedObservedPoint> observedPoints;
57
58 /** RMS for convergence.
59 * @since 10.3
60 */
61 private double convergenceRMS;
62
63 /** Maximum number of iterations.
64 * @since 10.3
65 */
66 private int maxIter;
67
68 /** Simple constructor.
69 * @param secularDegree degree of polynomial secular part
70 * @param pulsations pulsations of harmonic part
71 */
72 public SecularAndHarmonic(final int secularDegree, final double... pulsations) {
73 this.secularDegree = secularDegree;
74 this.pulsations = pulsations.clone();
75 this.observedPoints = new ArrayList<>();
76 this.convergenceRMS = 0.0;
77 this.maxIter = Integer.MAX_VALUE;
78 }
79
80 /** Reset fitting.
81 * @param date reference date
82 * @param initialGuess initial guess for the parameters
83 * @see #getReferenceDate()
84 */
85 public void resetFitting(final AbsoluteDate date, final double... initialGuess) {
86 reference = date;
87 fitted = initialGuess.clone();
88 observedPoints.clear();
89 }
90
91 /** Set RMS for convergence.
92 * <p>
93 * The RMS is the square-root of the sum of squared of
94 * the residuals, divided by the number of measurements.
95 * </p>
96 * @param convergenceRMS RMS below which convergence is considered to have been reached
97 * @since 10.3
98 */
99 public void setConvergenceRMS(final double convergenceRMS) {
100 this.convergenceRMS = convergenceRMS;
101 }
102
103 /** Set maximum number of iterations.
104 * @param maxIter maximum number of iterations
105 * @since 10.3
106 */
107 public void setMaxIter(final int maxIter) {
108 this.maxIter = maxIter;
109 }
110
111 /** Add a fitting point.
112 * <p>
113 * The point weight is set to 1.0
114 * </p>
115 * @param date date of the point
116 * @param osculatingValue osculating value
117 * @see #addWeightedPoint(AbsoluteDate, double, double)
118 */
119 public void addPoint(final AbsoluteDate date, final double osculatingValue) {
120 addWeightedPoint(date, osculatingValue, 1.0);
121 }
122
123 /** Add a weighted fitting point.
124 * @param date date of the point
125 * @param osculatingValue osculating value
126 * @param weight weight of the points
127 * @since 12.0
128 */
129 public void addWeightedPoint(final AbsoluteDate date, final double osculatingValue, final double weight) {
130 observedPoints.add(new WeightedObservedPoint(weight, date.durationFrom(reference), osculatingValue));
131 }
132
133 /** Get the reference date.
134 * @return reference date
135 * @see #resetFitting(AbsoluteDate, double...)
136 */
137 public AbsoluteDate getReferenceDate() {
138 return reference;
139 }
140
141 /** Get degree of polynomial secular part.
142 * @return degree of polynomial secular part
143 * @since 12.0
144 */
145 public int getSecularDegree() {
146 return secularDegree;
147 }
148
149 /** Get the pulsations of harmonic part.
150 * @return pulsations of harmonic part
151 * @since 12.0
152 */
153 public double[] getPulsations() {
154 return pulsations.clone();
155 }
156
157 /** Get an upper bound of the fitted harmonic amplitude.
158 * @return upper bound of the fitted harmonic amplitude
159 */
160 public double getHarmonicAmplitude() {
161 double amplitude = 0;
162 for (int i = 0; i < pulsations.length; ++i) {
163 amplitude += FastMath.hypot(fitted[secularDegree + 2 * i + 1],
164 fitted[secularDegree + 2 * i + 2]);
165 }
166 return amplitude;
167 }
168
169 /** Fit parameters.
170 * @see #getFittedParameters()
171 */
172 public void fit() {
173
174 final AbstractCurveFitter fitter = new AbstractCurveFitter() {
175 /** {@inheritDoc} */
176 @Override
177 protected LeastSquaresProblem getProblem(final Collection<WeightedObservedPoint> observations) {
178 // Prepare least-squares problem.
179 final int len = observations.size();
180 final double[] target = new double[len];
181 final double[] weights = new double[len];
182
183 int i = 0;
184 for (final WeightedObservedPoint obs : observations) {
185 target[i] = obs.getY();
186 weights[i] = obs.getWeight();
187 ++i;
188 }
189
190 final AbstractCurveFitter.TheoreticalValuesFunction model =
191 new AbstractCurveFitter.TheoreticalValuesFunction(new LocalParametricFunction(), observations);
192
193 // build a new least squares problem set up to fit a secular and harmonic curve to the observed points
194 return new LeastSquaresBuilder().
195 maxEvaluations(Integer.MAX_VALUE).
196 maxIterations(maxIter).
197 checker((iteration, previous, current) -> current.getRMS() <= convergenceRMS).
198 start(fitted).
199 target(target).
200 weight(new DiagonalMatrix(weights)).
201 model(model.getModelFunction(), model.getModelFunctionJacobian()).
202 build();
203
204 }
205 };
206
207 fitted = fitter.fit(observedPoints);
208
209 }
210
211 /** Local parametric function used for fitting. */
212 private class LocalParametricFunction implements ParametricUnivariateFunction {
213
214 /** {@inheritDoc} */
215 public double value(final double x, final double... parameters) {
216 return truncatedValue(secularDegree, pulsations.length, x, parameters);
217 }
218
219 /** {@inheritDoc} */
220 public double[] gradient(final double x, final double... parameters) {
221 final double[] gradient = new double[secularDegree + 1 + 2 * pulsations.length];
222
223 // secular part
224 double xN = 1.0;
225 for (int i = 0; i <= secularDegree; ++i) {
226 gradient[i] = xN;
227 xN *= x;
228 }
229
230 // harmonic part
231 for (int i = 0; i < pulsations.length; ++i) {
232 final SinCos sc = FastMath.sinCos(pulsations[i] * x);
233 gradient[secularDegree + 2 * i + 1] = sc.cos();
234 gradient[secularDegree + 2 * i + 2] = sc.sin();
235 }
236
237 return gradient;
238 }
239
240 }
241
242 /** Get a copy of the last fitted parameters.
243 * @return copy of the last fitted parameters.
244 * @see #fit()
245 */
246 public double[] getFittedParameters() {
247 return fitted.clone();
248 }
249
250 /** Get fitted osculating value.
251 * @param date current date
252 * @return osculating value at current date
253 */
254 public double osculatingValue(final AbsoluteDate date) {
255 return truncatedValue(secularDegree, pulsations.length,
256 date.durationFrom(reference), fitted);
257 }
258
259 /** Get fitted osculating derivative.
260 * @param date current date
261 * @return osculating derivative at current date
262 */
263 public double osculatingDerivative(final AbsoluteDate date) {
264 return truncatedDerivative(secularDegree, pulsations.length,
265 date.durationFrom(reference), fitted);
266 }
267
268 /** Get fitted osculating second derivative.
269 * @param date current date
270 * @return osculating second derivative at current date
271 */
272 public double osculatingSecondDerivative(final AbsoluteDate date) {
273 return truncatedSecondDerivative(secularDegree, pulsations.length,
274 date.durationFrom(reference), fitted);
275 }
276
277 /** Get mean value, truncated to first components.
278 * @param date current date
279 * @param degree degree of polynomial secular part to consider
280 * @param harmonics number of harmonics terms to consider
281 * @return mean value at current date
282 */
283 public double meanValue(final AbsoluteDate date, final int degree, final int harmonics) {
284 return truncatedValue(degree, harmonics, date.durationFrom(reference), fitted);
285 }
286
287 /** Get mean derivative, truncated to first components.
288 * @param date current date
289 * @param degree degree of polynomial secular part to consider
290 * @param harmonics number of harmonics terms to consider
291 * @return mean derivative at current date
292 */
293 public double meanDerivative(final AbsoluteDate date, final int degree, final int harmonics) {
294 return truncatedDerivative(degree, harmonics, date.durationFrom(reference), fitted);
295 }
296
297 /** Approximate an already fitted model to polynomial only terms.
298 * <p>
299 * This method is mainly used in order to combine the large amplitude long
300 * periods with the secular part as a new approximate polynomial model over
301 * some time range. This should be used rather than simply extracting the
302 * polynomial coefficients from {@link #getFittedParameters()} when some
303 * periodic terms amplitudes are large (for example Sun resonance effects
304 * on local solar time in sun synchronous orbits). In theses cases, the pure
305 * polynomial secular part in the coefficients may be far from the mean model.
306 * </p>
307 * @param combinedDegree desired degree for the combined polynomial
308 * @param combinedReference desired reference date for the combined polynomial
309 * @param meanDegree degree of polynomial secular part to consider
310 * @param meanHarmonics number of harmonics terms to consider
311 * @param start start date of the approximation time range
312 * @param end end date of the approximation time range
313 * @param step sampling step
314 * @return coefficients of the approximate polynomial (in increasing degree order),
315 * using the user provided reference date
316 */
317 public double[] approximateAsPolynomialOnly(final int combinedDegree, final AbsoluteDate combinedReference,
318 final int meanDegree, final int meanHarmonics,
319 final AbsoluteDate start, final AbsoluteDate end,
320 final double step) {
321 final List<WeightedObservedPoint> points = new ArrayList<>();
322 for (AbsoluteDate date = start; date.compareTo(end) < 0; date = date.shiftedBy(step)) {
323 points.add(new WeightedObservedPoint(1.0,
324 date.durationFrom(combinedReference),
325 meanValue(date, meanDegree, meanHarmonics)));
326 }
327 return PolynomialCurveFitter.create(combinedDegree).fit(points);
328 }
329
330 /** Get mean second derivative, truncated to first components.
331 * @param date current date
332 * @param degree degree of polynomial secular part
333 * @param harmonics number of harmonics terms to consider
334 * @return mean second derivative at current date
335 */
336 public double meanSecondDerivative(final AbsoluteDate date, final int degree, final int harmonics) {
337 return truncatedSecondDerivative(degree, harmonics, date.durationFrom(reference), fitted);
338 }
339
340 /** Get value truncated to first components.
341 * @param degree degree of polynomial secular part
342 * @param harmonics number of harmonics terms to consider
343 * @param time time parameter
344 * @param parameters models parameters (must include all parameters,
345 * including the ones ignored due to model truncation)
346 * @return truncated value
347 */
348 private double truncatedValue(final int degree, final int harmonics,
349 final double time, final double... parameters) {
350
351 double value = 0;
352
353 // secular part
354 double tN = 1.0;
355 for (int i = 0; i <= degree; ++i) {
356 value += parameters[i] * tN;
357 tN *= time;
358 }
359
360 // harmonic part
361 for (int i = 0; i < harmonics; ++i) {
362 final SinCos sc = FastMath.sinCos(pulsations[i] * time);
363 value += parameters[secularDegree + 2 * i + 1] * sc.cos() +
364 parameters[secularDegree + 2 * i + 2] * sc.sin();
365 }
366
367 return value;
368
369 }
370
371 /** Get derivative truncated to first components.
372 * @param degree degree of polynomial secular part
373 * @param harmonics number of harmonics terms to consider
374 * @param time time parameter
375 * @param parameters models parameters (must include all parameters,
376 * including the ones ignored due to model truncation)
377 * @return truncated derivative
378 */
379 private double truncatedDerivative(final int degree, final int harmonics,
380 final double time, final double... parameters) {
381
382 double derivative = 0;
383
384 // secular part
385 double tN = 1.0;
386 for (int i = 1; i <= degree; ++i) {
387 derivative += i * parameters[i] * tN;
388 tN *= time;
389 }
390
391 // harmonic part
392 for (int i = 0; i < harmonics; ++i) {
393 final SinCos sc = FastMath.sinCos(pulsations[i] * time);
394 derivative += pulsations[i] * (-parameters[secularDegree + 2 * i + 1] * sc.sin() +
395 parameters[secularDegree + 2 * i + 2] * sc.cos());
396 }
397
398 return derivative;
399
400 }
401
402 /** Get second derivative truncated to first components.
403 * @param degree degree of polynomial secular part
404 * @param harmonics number of harmonics terms to consider
405 * @param time time parameter
406 * @param parameters models parameters (must include all parameters,
407 * including the ones ignored due to model truncation)
408 * @return truncated second derivative
409 */
410 private double truncatedSecondDerivative(final int degree, final int harmonics,
411 final double time, final double... parameters) {
412
413 double d2 = 0;
414
415 // secular part
416 double tN = 1.0;
417 for (int i = 2; i <= degree; ++i) {
418 d2 += (i - 1) * i * parameters[i] * tN;
419 tN *= time;
420 }
421
422 // harmonic part
423 for (int i = 0; i < harmonics; ++i) {
424 final SinCos sc = FastMath.sinCos(pulsations[i] * time);
425 d2 += -pulsations[i] * pulsations[i] *
426 (parameters[secularDegree + 2 * i + 1] * sc.cos() +
427 parameters[secularDegree + 2 * i + 2] * sc.sin());
428 }
429
430 return d2;
431
432 }
433
434 }