1 /* Copyright 2002-2022 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<WeightedObservedPoint>();
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 * @param date date of the point
113 * @param osculatingValue osculating value
114 */
115 public void addPoint(final AbsoluteDate date, final double osculatingValue) {
116 observedPoints.add(new WeightedObservedPoint(1.0, date.durationFrom(reference), osculatingValue));
117 }
118
119 /** Get the reference date.
120 * @return reference date
121 * @see #resetFitting(AbsoluteDate, double...)
122 */
123 public AbsoluteDate getReferenceDate() {
124 return reference;
125 }
126
127 /** Get an upper bound of the fitted harmonic amplitude.
128 * @return upper bound of the fitted harmonic amplitude
129 */
130 public double getHarmonicAmplitude() {
131 double amplitude = 0;
132 for (int i = 0; i < pulsations.length; ++i) {
133 amplitude += FastMath.hypot(fitted[secularDegree + 2 * i + 1],
134 fitted[secularDegree + 2 * i + 2]);
135 }
136 return amplitude;
137 }
138
139 /** Fit parameters.
140 * @see #getFittedParameters()
141 */
142 public void fit() {
143
144 final AbstractCurveFitter fitter = new AbstractCurveFitter() {
145 /** {@inheritDoc} */
146 @Override
147 protected LeastSquaresProblem getProblem(final Collection<WeightedObservedPoint> observations) {
148 // Prepare least-squares problem.
149 final int len = observations.size();
150 final double[] target = new double[len];
151 final double[] weights = new double[len];
152
153 int i = 0;
154 for (final WeightedObservedPoint obs : observations) {
155 target[i] = obs.getY();
156 weights[i] = obs.getWeight();
157 ++i;
158 }
159
160 final AbstractCurveFitter.TheoreticalValuesFunction model =
161 new AbstractCurveFitter.TheoreticalValuesFunction(new LocalParametricFunction(), observations);
162
163 // build a new least squares problem set up to fit a secular and harmonic curve to the observed points
164 return new LeastSquaresBuilder().
165 maxEvaluations(Integer.MAX_VALUE).
166 maxIterations(maxIter).
167 checker((iteration, previous, current) -> current.getRMS() <= convergenceRMS).
168 start(fitted).
169 target(target).
170 weight(new DiagonalMatrix(weights)).
171 model(model.getModelFunction(), model.getModelFunctionJacobian()).
172 build();
173
174 }
175 };
176
177 fitted = fitter.fit(observedPoints);
178
179 }
180
181 /** Local parametric function used for fitting. */
182 private class LocalParametricFunction implements ParametricUnivariateFunction {
183
184 /** {@inheritDoc} */
185 public double value(final double x, final double... parameters) {
186 return truncatedValue(secularDegree, pulsations.length, x, parameters);
187 }
188
189 /** {@inheritDoc} */
190 public double[] gradient(final double x, final double... parameters) {
191 final double[] gradient = new double[secularDegree + 1 + 2 * pulsations.length];
192
193 // secular part
194 double xN = 1.0;
195 for (int i = 0; i <= secularDegree; ++i) {
196 gradient[i] = xN;
197 xN *= x;
198 }
199
200 // harmonic part
201 for (int i = 0; i < pulsations.length; ++i) {
202 final SinCos sc = FastMath.sinCos(pulsations[i] * x);
203 gradient[secularDegree + 2 * i + 1] = sc.cos();
204 gradient[secularDegree + 2 * i + 2] = sc.sin();
205 }
206
207 return gradient;
208 }
209
210 }
211
212 /** Get a copy of the last fitted parameters.
213 * @return copy of the last fitted parameters.
214 * @see #fit()
215 */
216 public double[] getFittedParameters() {
217 return fitted.clone();
218 }
219
220 /** Get fitted osculating value.
221 * @param date current date
222 * @return osculating value at current date
223 */
224 public double osculatingValue(final AbsoluteDate date) {
225 return truncatedValue(secularDegree, pulsations.length,
226 date.durationFrom(reference), fitted);
227 }
228
229 /** Get fitted osculating derivative.
230 * @param date current date
231 * @return osculating derivative at current date
232 */
233 public double osculatingDerivative(final AbsoluteDate date) {
234 return truncatedDerivative(secularDegree, pulsations.length,
235 date.durationFrom(reference), fitted);
236 }
237
238 /** Get fitted osculating second derivative.
239 * @param date current date
240 * @return osculating second derivative at current date
241 */
242 public double osculatingSecondDerivative(final AbsoluteDate date) {
243 return truncatedSecondDerivative(secularDegree, pulsations.length,
244 date.durationFrom(reference), fitted);
245 }
246
247 /** Get mean value, truncated to first components.
248 * @param date current date
249 * @param degree degree of polynomial secular part to consider
250 * @param harmonics number of harmonics terms to consider
251 * @return mean value at current date
252 */
253 public double meanValue(final AbsoluteDate date, final int degree, final int harmonics) {
254 return truncatedValue(degree, harmonics, date.durationFrom(reference), fitted);
255 }
256
257 /** Get mean derivative, truncated to first components.
258 * @param date current date
259 * @param degree degree of polynomial secular part to consider
260 * @param harmonics number of harmonics terms to consider
261 * @return mean derivative at current date
262 */
263 public double meanDerivative(final AbsoluteDate date, final int degree, final int harmonics) {
264 return truncatedDerivative(degree, harmonics, date.durationFrom(reference), fitted);
265 }
266
267 /** Approximate an already fitted model to polynomial only terms.
268 * <p>
269 * This method is mainly used in order to combine the large amplitude long
270 * periods with the secular part as a new approximate polynomial model over
271 * some time range. This should be used rather than simply extracting the
272 * polynomial coefficients from {@link #getFittedParameters()} when some
273 * periodic terms amplitudes are large (for example Sun resonance effects
274 * on local solar time in sun synchronous orbits). In theses cases, the pure
275 * polynomial secular part in the coefficients may be far from the mean model.
276 * </p>
277 * @param combinedDegree desired degree for the combined polynomial
278 * @param combinedReference desired reference date for the combined polynomial
279 * @param meanDegree degree of polynomial secular part to consider
280 * @param meanHarmonics number of harmonics terms to consider
281 * @param start start date of the approximation time range
282 * @param end end date of the approximation time range
283 * @param step sampling step
284 * @return coefficients of the approximate polynomial (in increasing degree order),
285 * using the user provided reference date
286 */
287 public double[] approximateAsPolynomialOnly(final int combinedDegree, final AbsoluteDate combinedReference,
288 final int meanDegree, final int meanHarmonics,
289 final AbsoluteDate start, final AbsoluteDate end,
290 final double step) {
291 final List<WeightedObservedPoint> points = new ArrayList<WeightedObservedPoint>();
292 for (AbsoluteDate date = start; date.compareTo(end) < 0; date = date.shiftedBy(step)) {
293 points.add(new WeightedObservedPoint(1.0,
294 date.durationFrom(combinedReference),
295 meanValue(date, meanDegree, meanHarmonics)));
296 }
297 return PolynomialCurveFitter.create(combinedDegree).fit(points);
298 }
299
300 /** Get mean second derivative, truncated to first components.
301 * @param date current date
302 * @param degree degree of polynomial secular part
303 * @param harmonics number of harmonics terms to consider
304 * @return mean second derivative at current date
305 */
306 public double meanSecondDerivative(final AbsoluteDate date, final int degree, final int harmonics) {
307 return truncatedSecondDerivative(degree, harmonics, date.durationFrom(reference), fitted);
308 }
309
310 /** Get value truncated to first components.
311 * @param degree degree of polynomial secular part
312 * @param harmonics number of harmonics terms to consider
313 * @param time time parameter
314 * @param parameters models parameters (must include all parameters,
315 * including the ones ignored due to model truncation)
316 * @return truncated value
317 */
318 private double truncatedValue(final int degree, final int harmonics,
319 final double time, final double... parameters) {
320
321 double value = 0;
322
323 // secular part
324 double tN = 1.0;
325 for (int i = 0; i <= degree; ++i) {
326 value += parameters[i] * tN;
327 tN *= time;
328 }
329
330 // harmonic part
331 for (int i = 0; i < harmonics; ++i) {
332 final SinCos sc = FastMath.sinCos(pulsations[i] * time);
333 value += parameters[secularDegree + 2 * i + 1] * sc.cos() +
334 parameters[secularDegree + 2 * i + 2] * sc.sin();
335 }
336
337 return value;
338
339 }
340
341 /** Get derivative truncated to first components.
342 * @param degree degree of polynomial secular part
343 * @param harmonics number of harmonics terms to consider
344 * @param time time parameter
345 * @param parameters models parameters (must include all parameters,
346 * including the ones ignored due to model truncation)
347 * @return truncated derivative
348 */
349 private double truncatedDerivative(final int degree, final int harmonics,
350 final double time, final double... parameters) {
351
352 double derivative = 0;
353
354 // secular part
355 double tN = 1.0;
356 for (int i = 1; i <= degree; ++i) {
357 derivative += i * parameters[i] * tN;
358 tN *= time;
359 }
360
361 // harmonic part
362 for (int i = 0; i < harmonics; ++i) {
363 final SinCos sc = FastMath.sinCos(pulsations[i] * time);
364 derivative += pulsations[i] * (-parameters[secularDegree + 2 * i + 1] * sc.sin() +
365 parameters[secularDegree + 2 * i + 2] * sc.cos());
366 }
367
368 return derivative;
369
370 }
371
372 /** Get second derivative truncated to first components.
373 * @param degree degree of polynomial secular part
374 * @param harmonics number of harmonics terms to consider
375 * @param time time parameter
376 * @param parameters models parameters (must include all parameters,
377 * including the ones ignored due to model truncation)
378 * @return truncated second derivative
379 */
380 private double truncatedSecondDerivative(final int degree, final int harmonics,
381 final double time, final double... parameters) {
382
383 double d2 = 0;
384
385 // secular part
386 double tN = 1.0;
387 for (int i = 2; i <= degree; ++i) {
388 d2 += (i - 1) * i * parameters[i] * tN;
389 tN *= time;
390 }
391
392 // harmonic part
393 for (int i = 0; i < harmonics; ++i) {
394 final SinCos sc = FastMath.sinCos(pulsations[i] * time);
395 d2 += -pulsations[i] * pulsations[i] *
396 (parameters[secularDegree + 2 * i + 1] * sc.cos() +
397 parameters[secularDegree + 2 * i + 2] * sc.sin());
398 }
399
400 return d2;
401
402 }
403
404 }