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.estimation.measurements;
18
19 import java.util.ArrayList;
20 import java.util.Collections;
21 import java.util.List;
22
23 import org.hipparchus.CalculusFieldElement;
24 import org.hipparchus.analysis.differentiation.Gradient;
25 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
26 import org.hipparchus.geometry.euclidean.threed.Vector3D;
27 import org.hipparchus.util.FastMath;
28 import org.orekit.frames.Frame;
29 import org.orekit.propagation.SpacecraftState;
30 import org.orekit.time.AbsoluteDate;
31 import org.orekit.time.FieldAbsoluteDate;
32 import org.orekit.utils.Constants;
33 import org.orekit.utils.FieldPVCoordinatesProvider;
34 import org.orekit.utils.PVCoordinatesProvider;
35 import org.orekit.utils.ParameterDriver;
36 import org.orekit.utils.FieldShiftingPVCoordinatesProvider;
37 import org.orekit.utils.ShiftingPVCoordinatesProvider;
38 import org.orekit.utils.TimeStampedFieldPVCoordinates;
39 import org.orekit.utils.TimeStampedPVCoordinates;
40
41 /** Abstract class handling measurements boilerplate.
42 * @param <T> the type of the measurement
43 * @author Luc Maisonobe
44 * @since 8.0
45 */
46 public abstract class AbstractMeasurement<T extends ObservedMeasurement<T>> implements ObservedMeasurement<T> {
47
48 /** List of the supported parameters. */
49 private final List<ParameterDriver> supportedParameters;
50
51 /** Satellites related to this measurement.
52 * @since 9.3
53 */
54 private final List<ObservableSatellite> satellites;
55
56 /** Date of the measurement. */
57 private final AbsoluteDate date;
58
59 /** Observed value. */
60 private double[] observed;
61
62 /** Theoretical standard deviation. */
63 private final double[] sigma;
64
65 /** Base weight. */
66 private final double[] baseWeight;
67
68 /** Modifiers that apply to the measurement.*/
69 private final List<EstimationModifier<T>> modifiers;
70
71 /** Enabling status. */
72 private boolean enabled;
73
74 /** Simple constructor for mono-dimensional measurements.
75 * <p>
76 * At construction, a measurement is enabled.
77 * </p>
78 * @param date date of the measurement
79 * @param observed observed value
80 * @param sigma theoretical standard deviation
81 * @param baseWeight base weight
82 * @param satellites satellites related to this measurement
83 * @since 9.3
84 */
85 protected AbstractMeasurement(final AbsoluteDate date, final double observed,
86 final double sigma, final double baseWeight,
87 final List<ObservableSatellite> satellites) {
88
89 this.supportedParameters = new ArrayList<>();
90
91 this.date = date;
92 this.observed = new double[] {
93 observed
94 };
95 this.sigma = new double[] {
96 sigma
97 };
98 this.baseWeight = new double[] {
99 baseWeight
100 };
101
102 this.satellites = satellites;
103
104 this.modifiers = new ArrayList<>();
105 setEnabled(true);
106
107 }
108
109 /** Simple constructor, for multi-dimensional measurements.
110 * <p>
111 * At construction, a measurement is enabled.
112 * </p>
113 * @param date date of the measurement
114 * @param observed observed value
115 * @param sigma theoretical standard deviation
116 * @param baseWeight base weight
117 * @param satellites satellites related to this measurement
118 * @since 9.3
119 */
120 protected AbstractMeasurement(final AbsoluteDate date, final double[] observed,
121 final double[] sigma, final double[] baseWeight,
122 final List<ObservableSatellite> satellites) {
123 this.supportedParameters = new ArrayList<>();
124
125 this.date = date;
126 this.observed = observed.clone();
127 this.sigma = sigma.clone();
128 this.baseWeight = baseWeight.clone();
129
130 this.satellites = satellites;
131
132 this.modifiers = new ArrayList<>();
133 setEnabled(true);
134
135 }
136
137 /** {@inheritDoc} */
138 @Override
139 public void setObservedValue(final double[] newObserved) {
140 this.observed = newObserved.clone();
141 }
142
143 /** Add a parameter driver.
144 * @param driver parameter driver to add
145 * @since 9.3
146 */
147 protected void addParameterDriver(final ParameterDriver driver) {
148 supportedParameters.add(driver);
149 }
150
151 /** {@inheritDoc} */
152 @Override
153 public List<ParameterDriver> getParametersDrivers() {
154 return Collections.unmodifiableList(supportedParameters);
155 }
156
157 /** {@inheritDoc} */
158 @Override
159 public void setEnabled(final boolean enabled) {
160 this.enabled = enabled;
161 }
162
163 /** {@inheritDoc} */
164 @Override
165 public boolean isEnabled() {
166 return enabled;
167 }
168
169 /** {@inheritDoc} */
170 @Override
171 public int getDimension() {
172 return observed.length;
173 }
174
175 /** {@inheritDoc} */
176 @Override
177 public double[] getTheoreticalStandardDeviation() {
178 return sigma.clone();
179 }
180
181 /** {@inheritDoc} */
182 @Override
183 public double[] getBaseWeight() {
184 return baseWeight.clone();
185 }
186
187 /** {@inheritDoc} */
188 @Override
189 public List<ObservableSatellite> getSatellites() {
190 return satellites;
191 }
192
193 /** Estimate the theoretical value without derivatives.
194 * The default implementation uses the computation with derivatives and ought to be overwritten for performance.
195 * <p>
196 * The theoretical value does not have <em>any</em> modifiers applied.
197 * </p>
198 * @param iteration iteration number
199 * @param evaluation evaluation number
200 * @param states orbital states at measurement date
201 * @return theoretical value
202 * @see #estimate(int, int, SpacecraftState[])
203 * @since 12.0
204 */
205 protected EstimatedMeasurementBase<T> theoreticalEvaluationWithoutDerivatives(final int iteration,
206 final int evaluation,
207 final SpacecraftState[] states) {
208 final EstimatedMeasurement<T> estimatedMeasurement = theoreticalEvaluation(iteration, evaluation, states);
209 final EstimatedMeasurementBase<T> estimatedMeasurementBase = new EstimatedMeasurementBase<>(estimatedMeasurement.getObservedMeasurement(),
210 iteration, evaluation, states, estimatedMeasurement.getParticipants());
211 estimatedMeasurementBase.setEstimatedValue(estimatedMeasurement.getEstimatedValue());
212 estimatedMeasurementBase.setStatus(estimatedMeasurement.getStatus());
213 return estimatedMeasurementBase;
214 }
215
216 /** Estimate the theoretical value.
217 * <p>
218 * The theoretical value does not have <em>any</em> modifiers applied.
219 * </p>
220 * @param iteration iteration number
221 * @param evaluation evaluation number
222 * @param states orbital states at measurement date
223 * @return theoretical value
224 * @see #estimate(int, int, SpacecraftState[])
225 */
226 protected abstract EstimatedMeasurement<T> theoreticalEvaluation(int iteration, int evaluation, SpacecraftState[] states);
227
228 /** {@inheritDoc} */
229 @Override
230 public EstimatedMeasurementBase<T> estimateWithoutDerivatives(final int iteration, final int evaluation, final SpacecraftState[] states) {
231
232 // compute the theoretical value
233 final EstimatedMeasurementBase<T> estimation = theoreticalEvaluationWithoutDerivatives(iteration, evaluation, states);
234
235 // apply the modifiers
236 for (final EstimationModifier<T> modifier : modifiers) {
237 modifier.modifyWithoutDerivatives(estimation);
238 }
239
240 return estimation;
241
242 }
243
244 /** {@inheritDoc} */
245 @Override
246 public EstimatedMeasurement<T> estimate(final int iteration, final int evaluation, final SpacecraftState[] states) {
247
248 // compute the theoretical value
249 final EstimatedMeasurement<T> estimation = theoreticalEvaluation(iteration, evaluation, states);
250
251 // apply the modifiers
252 for (final EstimationModifier<T> modifier : modifiers) {
253 modifier.modify(estimation);
254 }
255
256 return estimation;
257
258 }
259
260 /** {@inheritDoc} */
261 @Override
262 public AbsoluteDate getDate() {
263 return date;
264 }
265
266 /** {@inheritDoc} */
267 @Override
268 public double[] getObservedValue() {
269 return observed.clone();
270 }
271
272 /** {@inheritDoc} */
273 @Override
274 public void addModifier(final EstimationModifier<T> modifier) {
275
276 // combine the measurement parameters and the modifier parameters
277 supportedParameters.addAll(modifier.getParametersDrivers());
278
279 modifiers.add(modifier);
280
281 }
282
283 /** {@inheritDoc} */
284 @Override
285 public List<EstimationModifier<T>> getModifiers() {
286 return Collections.unmodifiableList(modifiers);
287 }
288
289 /** Compute propagation delay on a link leg (typically downlink or uplink).
290 * @param adjustableEmitterPV position/velocity of emitter that may be adjusted
291 * @param receiverPosition fixed position of receiver at {@code signalArrivalDate}
292 * @param frame inertial frame in which both {@code adjustableEmitterPV} and
293 * {@code receiverPosition} are defined
294 * @param signalArrivalDate date at which the signal arrives to receiver
295 * @return <em>positive</em> delay between signal emission and signal reception dates
296 * @since 13.0
297 */
298 public static double signalTimeOfFlightAdjustableEmitter(final TimeStampedPVCoordinates adjustableEmitterPV,
299 final Vector3D receiverPosition,
300 final AbsoluteDate signalArrivalDate,
301 final Frame frame) {
302 return signalTimeOfFlightAdjustableEmitter(new ShiftingPVCoordinatesProvider(adjustableEmitterPV, frame),
303 adjustableEmitterPV.getDate(),
304 receiverPosition, signalArrivalDate,
305 frame);
306 }
307
308 /** Compute propagation delay on a link leg (typically downlink or uplink).
309 * @param adjustableEmitter position/velocity provider of emitter
310 * @param approxEmissionDate approximate emission date
311 * @param receiverPosition fixed position of receiver at {@code signalArrivalDate}
312 * @param signalArrivalDate date at which the signal arrives to receiver
313 * @param frame inertial frame in which receiver is defined
314 * @return <em>positive</em> delay between signal emission and signal reception dates
315 * @since 13.0
316 */
317 public static double signalTimeOfFlightAdjustableEmitter(final PVCoordinatesProvider adjustableEmitter,
318 final AbsoluteDate approxEmissionDate,
319 final Vector3D receiverPosition,
320 final AbsoluteDate signalArrivalDate,
321 final Frame frame) {
322
323 // initialize emission date search loop assuming the state is already correct
324 // this will be true for all but the first orbit determination iteration,
325 // and even for the first iteration the loop will converge very fast
326 final double offset = signalArrivalDate.durationFrom(approxEmissionDate);
327 double delay = offset;
328
329 // search signal transit date, computing the signal travel in inertial frame
330 final double cReciprocal = 1.0 / Constants.SPEED_OF_LIGHT;
331 double delta;
332 int count = 0;
333 do {
334 final double previous = delay;
335 final Vector3D pos = adjustableEmitter.getPosition(approxEmissionDate.shiftedBy(offset - delay), frame);
336 delay = receiverPosition.distance(pos) * cReciprocal;
337 delta = FastMath.abs(delay - previous);
338 } while (count++ < 10 && delta >= 2 * FastMath.ulp(delay));
339
340 return delay;
341
342 }
343
344 /** Compute propagation delay on a link leg (typically downlink or uplink).
345 * @param emitterPosition fixed position of emitter
346 * @param emissionDate emission date
347 * @param adjustableReceiverPV position/velocity of receiver that may be adjusted
348 * @param approxReceptionDate approximate reception date
349 * @param frame inertial frame in which both {@code emitterPosition} and
350 * {@code adjustableReceiverPV} are defined
351 * @return <em>positive</em> delay between signal emission and signal reception dates
352 * @since 13.0
353 */
354 public static double signalTimeOfFlightAdjustableReceiver(final Vector3D emitterPosition,
355 final AbsoluteDate emissionDate,
356 final TimeStampedPVCoordinates adjustableReceiverPV,
357 final AbsoluteDate approxReceptionDate,
358 final Frame frame) {
359 return signalTimeOfFlightAdjustableReceiver(emitterPosition, emissionDate,
360 new ShiftingPVCoordinatesProvider(adjustableReceiverPV, frame),
361 approxReceptionDate, frame);
362 }
363
364 /** Compute propagation delay on a link leg (typically downlink or uplink).
365 * @param emitterPosition fixed position of emitter
366 * @param emissionDate emission date
367 * @param adjustableReceiver provider for adjusting receiver position
368 * @param approxReceptionDate approximate reception date
369 * @param frame inertial frame in which emitter is defined
370 * @return <em>positive</em> delay between signal emission and signal reception dates
371 * @since 13.0
372 */
373 public static double signalTimeOfFlightAdjustableReceiver(final Vector3D emitterPosition,
374 final AbsoluteDate emissionDate,
375 final PVCoordinatesProvider adjustableReceiver,
376 final AbsoluteDate approxReceptionDate,
377 final Frame frame) {
378
379 // initialize reception date search loop assuming the state is already correct
380 final double offset = approxReceptionDate.durationFrom(emissionDate);
381 double delay = offset;
382
383 // search signal transit date, computing the signal travel in inertial frame
384 final double cReciprocal = 1.0 / Constants.SPEED_OF_LIGHT;
385 double delta;
386 int count = 0;
387 do {
388 final double previous = delay;
389 final Vector3D arrivalP = adjustableReceiver.getPosition(approxReceptionDate.shiftedBy(delay - offset), frame);
390 delay = arrivalP.distance(emitterPosition) * cReciprocal;
391 delta = FastMath.abs(delay - previous);
392 count++;
393 } while (count < 10 && delta >= 2 * FastMath.ulp(delay));
394
395 return delay;
396
397 }
398
399 /** Compute propagation delay on a link leg (typically downlink or uplink).
400 * @param adjustableEmitterPV position/velocity of emitter that may be adjusted
401 * @param receiverPosition fixed position of receiver at {@code signalArrivalDate},
402 * in the same frame as {@code adjustableEmitterPV}
403 * @param signalArrivalDate date at which the signal arrives to receiver
404 * @return <em>positive</em> delay between signal emission and signal reception dates
405 * @param frame inertial frame in which both {@code adjustableEmitterPV} and
406 * {@code receiverPosition} are defined
407 * @param <T> the type of the components
408 * @since 13.0
409 */
410 public static <T extends CalculusFieldElement<T>> T signalTimeOfFlightAdjustableEmitter(final TimeStampedFieldPVCoordinates<T> adjustableEmitterPV,
411 final FieldVector3D<T> receiverPosition,
412 final FieldAbsoluteDate<T> signalArrivalDate,
413 final Frame frame) {
414 return signalTimeOfFlightAdjustableEmitter(new FieldShiftingPVCoordinatesProvider<>(adjustableEmitterPV,
415 frame),
416 adjustableEmitterPV.getDate(),
417 receiverPosition, signalArrivalDate,
418 frame);
419 }
420
421 /** Compute propagation delay on a link leg (typically downlink or uplink).
422 * @param adjustableEmitter position/velocity provider of emitter
423 * @param approxEmissionDate approximate emission date
424 * @param receiverPosition fixed position of receiver at {@code signalArrivalDate},
425 * in the same frame as {@code adjustableEmitterPV}
426 * @param signalArrivalDate date at which the signal arrives to receiver
427 * @param frame inertial frame in which receiver is defined
428 * @return <em>positive</em> delay between signal emission and signal reception dates
429 * @param <T> the type of the components
430 * @since 13.0
431 */
432 public static <T extends CalculusFieldElement<T>> T signalTimeOfFlightAdjustableEmitter(final FieldPVCoordinatesProvider<T> adjustableEmitter,
433 final FieldAbsoluteDate<T> approxEmissionDate,
434 final FieldVector3D<T> receiverPosition,
435 final FieldAbsoluteDate<T> signalArrivalDate,
436 final Frame frame) {
437
438 // Initialize emission date search loop assuming the emitter PV is almost correct
439 // this will be true for all but the first orbit determination iteration,
440 // and even for the first iteration the loop will converge extremely fast
441 final T offset = signalArrivalDate.durationFrom(approxEmissionDate);
442 T delay = offset;
443
444 // search signal transit date, computing the signal travel in the frame shared by emitter and receiver
445 final double cReciprocal = 1.0 / Constants.SPEED_OF_LIGHT;
446 double delta;
447 int count = 0;
448 do {
449 final double previous = delay.getReal();
450 final FieldVector3D<T> transitP = adjustableEmitter.getPosition(approxEmissionDate.shiftedBy(offset.subtract(delay)),
451 frame);
452 delay = receiverPosition.distance(transitP).multiply(cReciprocal);
453 delta = FastMath.abs(delay.getReal() - previous);
454 } while (count++ < 10 && delta >= 2 * FastMath.ulp(delay.getReal()));
455
456 return delay;
457
458 }
459
460 /** Compute propagation delay on a link leg (typically downlink or uplink).
461 * @param emitterPosition fixed position of emitter
462 * @param emissionDate emission date
463 * @param adjustableReceiverPV position/velocity of emitter that may be adjusted
464 * @param approxReceptionDate approximate reception date
465 * @param frame inertial frame in which both {@code emitterPosition} and
466 * {@code adjustableReceiverPV} are defined
467 * @param <T> the type of the components
468 * @return <em>positive</em> delay between signal emission and signal reception dates
469 * @since 13.0
470 */
471 public static <T extends CalculusFieldElement<T>> T signalTimeOfFlightAdjustableReceiver(final FieldVector3D<T> emitterPosition,
472 final FieldAbsoluteDate<T> emissionDate,
473 final TimeStampedFieldPVCoordinates<T> adjustableReceiverPV,
474 final FieldAbsoluteDate<T> approxReceptionDate,
475 final Frame frame) {
476 return signalTimeOfFlightAdjustableReceiver(emitterPosition, emissionDate,
477 new FieldShiftingPVCoordinatesProvider<>(adjustableReceiverPV, frame),
478 approxReceptionDate, frame);
479 }
480
481 /** Compute propagation delay on a link leg (typically downlink or uplink).
482 * @param emitterPosition fixed position of emitter
483 * @param emissionDate emission date
484 * @param adjustableReceiver provider for adjusting receiver position
485 * @param approxReceptionDate approximate reception date
486 * @param frame inertial frame in which emitter is defined
487 * @param <T> the type of the components
488 * @return <em>positive</em> delay between signal emission and signal reception dates
489 * @since 13.0
490 */
491 public static <T extends CalculusFieldElement<T>> T signalTimeOfFlightAdjustableReceiver(final FieldVector3D<T> emitterPosition,
492 final FieldAbsoluteDate<T> emissionDate,
493 final FieldPVCoordinatesProvider<T> adjustableReceiver,
494 final FieldAbsoluteDate<T> approxReceptionDate,
495 final Frame frame) {
496
497 // initialize reception date search loop assuming the state is already correct
498 final T offset = approxReceptionDate.durationFrom(emissionDate);
499 T delay = offset;
500
501 // search signal transit date, computing the signal travel in the frame shared by emitter and receiver
502 final double cReciprocal = 1.0 / Constants.SPEED_OF_LIGHT;
503 double delta;
504 int count = 0;
505 do {
506 final double previous = delay.getReal();
507 final FieldVector3D<T> arrivalP = adjustableReceiver.getPosition(approxReceptionDate.shiftedBy(delay.subtract(offset)),
508 frame);
509 delay = arrivalP.distance(emitterPosition).multiply(cReciprocal);
510 delta = FastMath.abs(delay.getReal() - previous);
511 } while (count++ < 10 && delta >= 2 * FastMath.ulp(delay.getReal()));
512
513 return delay;
514
515 }
516
517 /** Get Cartesian coordinates as derivatives.
518 * <p>
519 * The position will correspond to variables {@code firstDerivative},
520 * {@code firstDerivative + 1} and {@code firstDerivative + 2}.
521 * The velocity will correspond to variables {@code firstDerivative + 3},
522 * {@code firstDerivative + 4} and {@code firstDerivative + 5}.
523 * The acceleration will correspond to constants.
524 * </p>
525 * @param state state of the satellite considered
526 * @param firstDerivative index of the first derivative
527 * @param freeParameters total number of free parameters in the gradient
528 * @return Cartesian coordinates as derivatives
529 * @since 10.2
530 */
531 public static TimeStampedFieldPVCoordinates<Gradient> getCoordinates(final SpacecraftState state,
532 final int firstDerivative,
533 final int freeParameters) {
534
535 // Position of the satellite expressed as a gradient
536 // The components of the position are the 3 first derivative parameters
537 final Vector3D p = state.getPosition();
538 final FieldVector3D<Gradient> pDS =
539 new FieldVector3D<>(Gradient.variable(freeParameters, firstDerivative, p.getX()),
540 Gradient.variable(freeParameters, firstDerivative + 1, p.getY()),
541 Gradient.variable(freeParameters, firstDerivative + 2, p.getZ()));
542
543 // Velocity of the satellite expressed as a gradient
544 // The components of the velocity are the 3 second derivative parameters
545 final Vector3D v = state.getPVCoordinates().getVelocity();
546 final FieldVector3D<Gradient> vDS =
547 new FieldVector3D<>(Gradient.variable(freeParameters, firstDerivative + 3, v.getX()),
548 Gradient.variable(freeParameters, firstDerivative + 4, v.getY()),
549 Gradient.variable(freeParameters, firstDerivative + 5, v.getZ()));
550
551 // Acceleration of the satellite
552 // The components of the acceleration are not derivative parameters
553 final Vector3D a = state.getPVCoordinates().getAcceleration();
554 final FieldVector3D<Gradient> aDS =
555 new FieldVector3D<>(Gradient.constant(freeParameters, a.getX()),
556 Gradient.constant(freeParameters, a.getY()),
557 Gradient.constant(freeParameters, a.getZ()));
558
559 return new TimeStampedFieldPVCoordinates<>(state.getDate(), pDS, vDS, aDS);
560
561 }
562
563 }