1 /* Copyright 2022-2025 Thales Alenia Space
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.analytical.gnss.data;
18
19 import org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.analysis.differentiation.FieldGradient;
22 import org.hipparchus.analysis.differentiation.FieldGradientField;
23 import org.hipparchus.analysis.differentiation.Gradient;
24 import org.hipparchus.util.FastMath;
25 import org.hipparchus.util.MathArrays;
26 import org.orekit.utils.Constants;
27 import org.orekit.utils.ParameterDriver;
28
29 import java.util.Arrays;
30 import java.util.List;
31
32 /** Factory for non-Keplerian drivers.
33 * @since 14.0
34 */
35 public class NonKeplerianDriversFactory {
36
37 /** Name for time parameter. */
38 public static final String TIME = "GnssTime";
39
40 /** Name for change rate in semi-major axis parameter. */
41 public static final String A_DOT = "GnssADot";
42
43 /** Name for delta of satellite mean motion. */
44 public static final String DELTA_N0 = "GnssDeltaN0";
45
46 /** Name for change rate in Δn₀. */
47 public static final String DELTA_N0_DOT = "GnssDeltaN0Dot";
48
49 /** Name for inclination rate parameter. */
50 public static final String INCLINATION_RATE = "GnssInclinationRate";
51
52 /** Name for longitude rate parameter. */
53 public static final String LONGITUDE_RATE = "GnssLongitudeRate";
54
55 /** Name for cosine of latitude argument harmonic parameter. */
56 public static final String LATITUDE_COSINE = "GnssLatitudeCosine";
57
58 /** Name for sine of latitude argument harmonic parameter. */
59 public static final String LATITUDE_SINE = "GnssLatitudeSine";
60
61 /** Name for cosine of orbit radius harmonic parameter. */
62 public static final String RADIUS_COSINE = "GnssRadiusCosine";
63
64 /** Name for sine of orbit radius harmonic parameter. */
65 public static final String RADIUS_SINE = "GnssRadiusSine";
66
67 /** Name for cosine of inclination harmonic parameter. */
68 public static final String INCLINATION_COSINE = "GnssInclinationCosine";
69
70 /** Name for sine of inclination harmonic parameter. */
71 public static final String INCLINATION_SINE = "GnssInclinationSine";
72
73 /** Name for zero-th order clock correction parameter. */
74 public static final String AF0 = "GnssClock0";
75
76 /** Name for first order clock correction parameter. */
77 public static final String AF1 = "GnssClock1";
78
79 /** Name for second order clock correction parameter. */
80 public static final String AF2 = "GnssClock2";
81
82 /** Index of time in the list returned by {@link #getParametersDrivers()}. */
83 public static final int TIME_INDEX = 0;
84
85 /** Index of change rate in semi-major axis parameter in the list returned by {@link #getParametersDrivers()}. */
86 public static final int A_DOT_INDEX = TIME_INDEX + 1;
87
88 /** Index of delta of satellite mean motion in the list returned by {@link #getParametersDrivers()}. */
89 public static final int DELTA_N0_INDEX = A_DOT_INDEX + 1;
90
91 /** Index of change rate in Δn₀ in the list returned by {@link #getParametersDrivers()}. */
92 public static final int DELTA_N0_DOT_INDEX = DELTA_N0_INDEX + 1;
93
94 /** Index of inclination rate in the list returned by {@link #getParametersDrivers()}. */
95 public static final int I_DOT_INDEX = DELTA_N0_DOT_INDEX + 1;
96
97 /** Index of longitude rate in the list returned by {@link #getParametersDrivers()}. */
98 public static final int OMEGA_DOT_INDEX = I_DOT_INDEX + 1;
99
100 /** Index of cosine on latitude argument in the list returned by {@link #getParametersDrivers()}. */
101 public static final int CUC_INDEX = OMEGA_DOT_INDEX + 1;
102
103 /** Index of sine on latitude argument in the list returned by {@link #getParametersDrivers()}. */
104 public static final int CUS_INDEX = CUC_INDEX + 1;
105
106 /** Index of cosine on radius in the list returned by {@link #getParametersDrivers()}. */
107 public static final int CRC_INDEX = CUS_INDEX + 1;
108
109 /** Index of sine on radius in the list returned by {@link #getParametersDrivers()}. */
110 public static final int CRS_INDEX = CRC_INDEX + 1;
111
112 /** Index of cosine on inclination in the list returned by {@link #getParametersDrivers()}. */
113 public static final int CIC_INDEX = CRS_INDEX + 1;
114
115 /** Index of sine on inclination in the list returned by {@link #getParametersDrivers()}. */
116 public static final int CIS_INDEX = CIC_INDEX + 1;
117
118 /** Index of zero-th order clock correction in the list returned by {@link #getParametersDrivers()}. */
119 public static final int AF0_INDEX = CIS_INDEX + 1;
120
121 /** Index of first order clock correction in the list returned by {@link #getParametersDrivers()}. */
122 public static final int AF1_INDEX = AF0_INDEX + 1;
123
124 /** Index of second order clock correction in the list returned by {@link #getParametersDrivers()}. */
125 public static final int AF2_INDEX = AF1_INDEX + 1;
126
127 /** Size of parameters array. */
128 public static final int SIZE = AF2_INDEX + 1;
129
130 /** Reference time. */
131 private final ParameterDriver timeDriver;
132
133 /** Change rate in semi-major axis (m/s). */
134 private final ParameterDriver aDotDriver;
135
136 /** Delta of satellite mean motion. */
137 private final ParameterDriver deltaN0Driver;
138
139 /** Change rate in Δn₀. */
140 private final ParameterDriver deltaN0DotDriver;
141
142 /** Inclination rate (rad/s). */
143 private final ParameterDriver iDotDriver;
144
145 /** Rate of right ascension (rad/s). */
146 private final ParameterDriver domDriver;
147
148 /** Amplitude of the cosine harmonic correction term to the argument of latitude. */
149 private final ParameterDriver cucDriver;
150
151 /** Amplitude of the sine harmonic correction term to the argument of latitude. */
152 private final ParameterDriver cusDriver;
153
154 /** Amplitude of the cosine harmonic correction term to the orbit radius. */
155 private final ParameterDriver crcDriver;
156
157 /** Amplitude of the sine harmonic correction term to the orbit radius. */
158 private final ParameterDriver crsDriver;
159
160 /** Amplitude of the cosine harmonic correction term to the inclination. */
161 private final ParameterDriver cicDriver;
162
163 /** Amplitude of the sine harmonic correction term to the inclination. */
164 private final ParameterDriver cisDriver;
165
166 /** SV zero-th order clock correction (s). */
167 private final ParameterDriver af0Driver;
168
169 /** SV first order clock correction (s/s). */
170 private final ParameterDriver af1Driver;
171
172 /** SV second order clock correction (s/s²). */
173 private final ParameterDriver af2Driver;
174
175 /** Simple constructor.
176 */
177 public NonKeplerianDriversFactory() {
178
179 // propagation drivers
180 this.timeDriver = new ParameterDriver(TIME, 0.0, FastMath.scalb(1.0, -10),
181 0, 7 * Constants.JULIAN_DAY);
182 this.aDotDriver = new ParameterDriver(A_DOT, 0.0, FastMath.scalb(1.0, -10),
183 Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
184 this.deltaN0Driver = new ParameterDriver(DELTA_N0, 0.0, FastMath.scalb(1.0, -36),
185 Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
186 this.deltaN0DotDriver = new ParameterDriver(DELTA_N0_DOT, 0.0, FastMath.scalb(1.0, -46),
187 Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
188 this.iDotDriver = new ParameterDriver(INCLINATION_RATE, 0.0, FastMath.scalb(1.0, -34),
189 Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
190 this.domDriver = new ParameterDriver(LONGITUDE_RATE, 0.0, FastMath.scalb(1.0, -34),
191 Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
192 this.cucDriver = new ParameterDriver(LATITUDE_COSINE, 0.0, FastMath.scalb(1.0, -24),
193 Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
194 this.cusDriver = new ParameterDriver(LATITUDE_SINE, 0.0, FastMath.scalb(1.0, -24),
195 Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
196 this.crcDriver = new ParameterDriver(RADIUS_COSINE, 0.0, FastMath.scalb(1.0, 0),
197 Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
198 this.crsDriver = new ParameterDriver(RADIUS_SINE, 0.0, FastMath.scalb(1.0, 0),
199 Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
200 this.cicDriver = new ParameterDriver(INCLINATION_COSINE, 0.0, FastMath.scalb(1.0, -24),
201 Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
202 this.cisDriver = new ParameterDriver(INCLINATION_SINE, 0.0, FastMath.scalb(1.0, -24),
203 Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
204
205 // clock drivers
206 this.af0Driver = new ParameterDriver(AF0, 0.0, FastMath.scalb(1.0, -26),
207 Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
208 this.af1Driver = new ParameterDriver(AF1, 0.0, FastMath.scalb(1.0, -42),
209 Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
210 this.af2Driver = new ParameterDriver(AF2, 0.0, FastMath.scalb(1.0, -58),
211 Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
212
213 }
214
215 /** Reset the parameters drivers from existing elements.
216 * @param elements elements to use for reset
217 */
218 public void reset(final GNSSOrbitalElements<?> elements) {
219 reset(timeDriver, elements.getTimeOfEphemeris().getSecondsInWeek());
220 reset(aDotDriver, elements.getADot());
221 reset(deltaN0Driver, elements.getDeltaN0());
222 reset(deltaN0DotDriver, elements.getDeltaN0Dot());
223 reset(iDotDriver, elements.getIDot());
224 reset(domDriver, elements.getOmegaDot());
225 reset(cucDriver, elements.getCuc());
226 reset(cusDriver, elements.getCus());
227 reset(crcDriver, elements.getCrc());
228 reset(crsDriver, elements.getCrs());
229 reset(cicDriver, elements.getCic());
230 reset(cisDriver, elements.getCis());
231 reset(af0Driver, elements.getAf0());
232 reset(af1Driver, elements.getAf1());
233 reset(af2Driver, elements.getAf2());
234 }
235
236 /** Reset the parameters drivers from existing elements.
237 * @param elements elements to use for reset
238 */
239 public void reset(final FieldGnssOrbitalElements<?, ?> elements) {
240 reset(timeDriver, elements.getTimeOfEphemeris().getGnssDate().getSecondsInWeek());
241 reset(aDotDriver, elements.getADot().getReal());
242 reset(deltaN0Driver, elements.getDeltaN0().getReal());
243 reset(deltaN0DotDriver, elements.getDeltaN0Dot().getReal());
244 reset(iDotDriver, elements.getIDot().getReal());
245 reset(domDriver, elements.getOmegaDot().getReal());
246 reset(cucDriver, elements.getCuc().getReal());
247 reset(cusDriver, elements.getCus().getReal());
248 reset(crcDriver, elements.getCrc().getReal());
249 reset(crsDriver, elements.getCrs().getReal());
250 reset(cicDriver, elements.getCic().getReal());
251 reset(cisDriver, elements.getCis().getReal());
252 reset(af0Driver, elements.getAf0().getReal());
253 reset(af1Driver, elements.getAf1().getReal());
254 reset(af2Driver, elements.getAf2().getReal());
255 }
256
257 /** Reset one driver.
258 * @param driver driver to reset
259 * @param value new value (also used as reference)
260 */
261 private void reset (final ParameterDriver driver, final double value) {
262 driver.setValue(value);
263 driver.setReferenceValue(value);
264 }
265
266 /** Get the 15 drivers for the non-Keplerian parameters.
267 * <p>
268 * Only the 15 non-Keplerian parameters (12 evolution parameters and 3 clock parameters)
269 * are listed here:
270 * Time driver at index {@link #TIME_INDEX},
271 * ADot driver at index {@link #A_DOT_INDEX},
272 * DeltaN0 driver at index {@link #DELTA_N0_INDEX},
273 * DeltaN0Dot driver at index {@link #DELTA_N0_DOT_INDEX},
274 * IDot driver at index {@link #I_DOT_INDEX},
275 * OmegaDot driver at index {@link #OMEGA_DOT_INDEX},
276 * Cuc driver at index {@link #CUC_INDEX},
277 * Cus driver at index {@link #CUS_INDEX},
278 * Crc driver at index {@link #CRC_INDEX},
279 * Crs driver at index {@link #CRS_INDEX},
280 * Cic driver at index {@link #CIC_INDEX},
281 * Cis driver at index {@link #CIS_INDEX},
282 * af0 driver at index {@link #AF0_INDEX},
283 * af1 driver at index {@link #AF1_INDEX},
284 * and af2 driver at index {@link #AF2_INDEX}.
285 * </p>
286 * @return 15 drivers for the non-Keplerian parameters
287 */
288 public List<ParameterDriver> getParametersDrivers() {
289
290 // ensure the parameters are really at the advertised indices
291 final ParameterDriver[] array = new ParameterDriver[SIZE];
292
293 array[TIME_INDEX] = timeDriver;
294 array[A_DOT_INDEX] = aDotDriver;
295 array[DELTA_N0_INDEX] = deltaN0Driver;
296 array[DELTA_N0_DOT_INDEX] = deltaN0DotDriver;
297 array[I_DOT_INDEX] = iDotDriver;
298 array[OMEGA_DOT_INDEX] = domDriver;
299 array[CUC_INDEX] = cucDriver;
300 array[CUS_INDEX] = cusDriver;
301 array[CRC_INDEX] = crcDriver;
302 array[CRS_INDEX] = crsDriver;
303 array[CIC_INDEX] = cicDriver;
304 array[CIS_INDEX] = cisDriver;
305
306 array[AF0_INDEX] = af0Driver;
307 array[AF1_INDEX] = af1Driver;
308 array[AF2_INDEX] = af2Driver;
309
310 return Arrays.asList(array);
311
312 }
313
314 /** Get driver for reference time of the GNSS orbit as a duration from week start.
315 * @return driver for reference time of the GNSS orbit (s)
316 */
317 public ParameterDriver getTimeDriver() {
318 return timeDriver;
319 }
320
321 /** Get driver for change rate in semi-major axis.
322 * @return driver for the change rate in semi-major axis
323 */
324 public ParameterDriver getADotDriver() {
325 return aDotDriver;
326 }
327
328 /** Get driver for the delta of satellite mean motion.
329 * @return driver for the delta of satellite mean motion
330 */
331 public ParameterDriver getDeltaN0Driver() {
332 return deltaN0Driver;
333 }
334
335 /** Get driver for the change rate in Δn₀.
336 * @return driver for change rate in Δn₀
337 */
338 public ParameterDriver getDeltaN0DotDriver() {
339 return deltaN0DotDriver;
340 }
341
342 /** Get driver for rate of inclination angle.
343 * @return driver for rate of inclination angle (rad/s)
344 */
345 public ParameterDriver getIDotDriver() {
346 return iDotDriver;
347 }
348
349 /** Get driver for rate of right ascension.
350 * @return driver for rate of right ascension (rad/s)
351 */
352 public ParameterDriver getOmegaDotDriver() {
353 return domDriver;
354 }
355
356 /** Get driver for amplitude of the cosine harmonic correction term to the argument of latitude.
357 * @return driver for amplitude of the cosine harmonic correction term to the argument of latitude (rad)
358 */
359 public ParameterDriver getCucDriver() {
360 return cucDriver;
361 }
362
363 /** Get driver for amplitude of the sine harmonic correction term to the argument of latitude.
364 * @return driver for amplitude of the sine harmonic correction term to the argument of latitude (rad)
365 */
366 public ParameterDriver getCusDriver() {
367 return cusDriver;
368 }
369
370 /** Get driver for amplitude of the cosine harmonic correction term to the orbit radius.
371 * @return driver for amplitude of the cosine harmonic correction term to the orbit radius (m)
372 */
373 public ParameterDriver getCrcDriver() {
374 return crcDriver;
375 }
376
377 /** Get driver for amplitude of the sine harmonic correction term to the orbit radius.
378 * @return driver for amplitude of the sine harmonic correction term to the orbit radius (m)
379 */
380 public ParameterDriver getCrsDriver() {
381 return crsDriver;
382 }
383
384 /** Get driver for amplitude of the cosine harmonic correction term to the angle of inclination.
385 * @return driver for amplitude of the cosine harmonic correction term to the angle of inclination (rad)
386 */
387 public ParameterDriver getCicDriver() {
388 return cicDriver;
389 }
390
391 /** Get driver for amplitude of the sine harmonic correction term to the angle of inclination.
392 * @return driver for amplitude of the sine harmonic correction term to the angle of inclination (rad)
393 */
394 public ParameterDriver getCisDriver() {
395 return cisDriver;
396 }
397
398 /** Get driver for SV zero-th order clock correction.
399 * @return driver for SV zero-th order clock correction (s)
400 */
401 public ParameterDriver getAf0Driver() {
402 return af0Driver;
403 }
404
405 /** Get driver for SV first order clock correction.
406 * @return driver for SV first order clock correction (s/s)
407 */
408 public ParameterDriver getAf1Driver() {
409 return af1Driver;
410 }
411
412 /** Get driver for SV second order clock correction.
413 * @return driver for SV second order clock correction (s/s²)
414 */
415 public ParameterDriver getAf2Driver() {
416 return af2Driver;
417 }
418
419 /** Get the non-Keplerian elements as gradient variables or constants, depending on selection status.
420 * @param freeParameters total number of free parameters in the gradient
421 * @return non-Keplerian elements as gradient variables or constants
422 */
423 public Gradient[] toGradients(final int freeParameters) {
424 final Filler filler = new Filler(freeParameters);
425 filler.manage(timeDriver, TIME_INDEX);
426 filler.manage(aDotDriver, A_DOT_INDEX);
427 filler.manage(deltaN0Driver, DELTA_N0_INDEX);
428 filler.manage(deltaN0DotDriver, DELTA_N0_DOT_INDEX);
429 filler.manage(iDotDriver, I_DOT_INDEX);
430 filler.manage(domDriver, OMEGA_DOT_INDEX);
431 filler.manage(cucDriver, CUC_INDEX);
432 filler.manage(cusDriver, CUS_INDEX);
433 filler.manage(crcDriver, CRC_INDEX);
434 filler.manage(crsDriver, CRS_INDEX);
435 filler.manage(cicDriver, CIC_INDEX);
436 filler.manage(cisDriver, CIS_INDEX);
437 filler.manage(af0Driver, AF0_INDEX);
438 filler.manage(af1Driver, AF1_INDEX);
439 filler.manage(af2Driver, AF2_INDEX);
440 return filler.gradients;
441 }
442
443 /** Get the non-Keplerian elements as gradient variables or constants, depending on selection status.
444 * @param <T> type of the field elements
445 * @param field field
446 * @param freeParameters total number of free parameters in the gradient
447 * @return non-Keplerian elements as gradient variables or constants
448 */
449 public <T extends CalculusFieldElement<T>> FieldGradient<T>[] toGradients(final Field<T> field,
450 final int freeParameters) {
451 final FieldFiller<T> filler = new FieldFiller<>(field, freeParameters);
452 filler.manage(timeDriver, TIME_INDEX);
453 filler.manage(aDotDriver, A_DOT_INDEX);
454 filler.manage(deltaN0Driver, DELTA_N0_INDEX);
455 filler.manage(deltaN0DotDriver, DELTA_N0_DOT_INDEX);
456 filler.manage(iDotDriver, I_DOT_INDEX);
457 filler.manage(domDriver, OMEGA_DOT_INDEX);
458 filler.manage(cucDriver, CUC_INDEX);
459 filler.manage(cusDriver, CUS_INDEX);
460 filler.manage(crcDriver, CRC_INDEX);
461 filler.manage(crsDriver, CRS_INDEX);
462 filler.manage(cicDriver, CIC_INDEX);
463 filler.manage(cisDriver, CIS_INDEX);
464 filler.manage(af0Driver, AF0_INDEX);
465 filler.manage(af1Driver, AF1_INDEX);
466 filler.manage(af2Driver, AF2_INDEX);
467 return filler.gradients;
468 }
469
470 /** Array filler for gradients.
471 * @since 14.0
472 */
473 private static class Filler {
474
475 /** Total number of free parameters in the gradient. */
476 private final int freeParameters;
477
478 /** Gradient array. */
479 private final Gradient[] gradients;
480
481 /** Partial derivative index. */
482 private int derivative;
483
484 /** Simple constructor.
485 * @param freeParameters total number of free parameters in the gradient
486 */
487 Filler(final int freeParameters) {
488 this.freeParameters = freeParameters;
489 this.gradients = new Gradient[SIZE];
490 this.derivative = 6;
491 }
492
493 /** Manage one driver.
494 * @param driver driver to manage
495 * @param index index of the driver in the array
496 */
497 private void manage(final ParameterDriver driver, final int index) {
498 if (driver.isSelected()) {
499 // this driver should be managed as a variable
500 gradients[index] = Gradient.variable(freeParameters, derivative, driver.getValue());
501 ++derivative;
502 } else {
503 // this driver should be managed as a constant
504 gradients[index] = Gradient.constant(freeParameters, driver.getValue());
505 }
506 }
507
508 }
509
510 /** Array filler for gradients.
511 * @param <T> field to which elements belong
512 * @since 14.0
513 */
514 private static class FieldFiller<T extends CalculusFieldElement<T>> {
515
516 /** Field. */
517 private final Field<T> field;
518
519 /** Total number of free parameters in the gradient. */
520 private final int freeParameters;
521
522 /** Gradient array. */
523 private final FieldGradient<T>[] gradients;
524
525 /** Partial derivative index. */
526 private int derivative;
527
528 /** Simple constructor.
529 * @param field field
530 * @param freeParameters total number of free parameters in the gradient
531 */
532 FieldFiller(final Field<T> field, final int freeParameters) {
533 this.field = field;
534 this.freeParameters = freeParameters;
535 this.gradients = MathArrays.buildArray(FieldGradientField.getField(field, freeParameters), SIZE);
536 this.derivative = 6;
537 }
538
539 /** Manage one driver.
540 * @param driver driver to manage
541 * @param index index of the driver in the array
542 */
543 private void manage(final ParameterDriver driver, final int index) {
544 if (driver.isSelected()) {
545 // this driver should be managed as a variable
546 gradients[index] = FieldGradient.variable(freeParameters, derivative,
547 field.getZero().newInstance(driver.getValue()));
548 ++derivative;
549 } else {
550 // this driver should be managed as a constant
551 gradients[index] = FieldGradient.constant(freeParameters,
552 field.getZero().newInstance(driver.getValue()));
553 }
554 }
555
556 }
557
558 }