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.AbsolutePVCoordinates;
34 import org.orekit.utils.FieldAbsolutePVCoordinates;
35 import org.orekit.utils.FieldPVCoordinatesProvider;
36 import org.orekit.utils.PVCoordinatesProvider;
37 import org.orekit.utils.ParameterDriver;
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 AbsolutePVCoordinates(frame, adjustableEmitterPV),
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 (PVCoordinatesProvider) new AbsolutePVCoordinates(frame, adjustableReceiverPV),
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 FieldAbsolutePVCoordinates<>(frame, adjustableEmitterPV),
415 adjustableEmitterPV.getDate(),
416 receiverPosition, signalArrivalDate,
417 frame);
418 }
419
420 /** Compute propagation delay on a link leg (typically downlink or uplink).
421 * @param adjustableEmitter position/velocity provider of emitter
422 * @param approxEmissionDate approximate emission date
423 * @param receiverPosition fixed position of receiver at {@code signalArrivalDate},
424 * in the same frame as {@code adjustableEmitterPV}
425 * @param signalArrivalDate date at which the signal arrives to receiver
426 * @param frame inertial frame in which receiver is defined
427 * @return <em>positive</em> delay between signal emission and signal reception dates
428 * @param <T> the type of the components
429 * @since 13.0
430 */
431 public static <T extends CalculusFieldElement<T>> T signalTimeOfFlightAdjustableEmitter(final FieldPVCoordinatesProvider<T> adjustableEmitter,
432 final FieldAbsoluteDate<T> approxEmissionDate,
433 final FieldVector3D<T> receiverPosition,
434 final FieldAbsoluteDate<T> signalArrivalDate,
435 final Frame frame) {
436
437 // Initialize emission date search loop assuming the emitter PV is almost correct
438 // this will be true for all but the first orbit determination iteration,
439 // and even for the first iteration the loop will converge extremely fast
440 final T offset = signalArrivalDate.durationFrom(approxEmissionDate);
441 T delay = offset;
442
443 // search signal transit date, computing the signal travel in the frame shared by emitter and receiver
444 final double cReciprocal = 1.0 / Constants.SPEED_OF_LIGHT;
445 double delta;
446 int count = 0;
447 do {
448 final double previous = delay.getReal();
449 final FieldVector3D<T> transitP = adjustableEmitter.getPosition(approxEmissionDate.shiftedBy(offset.subtract(delay)),
450 frame);
451 delay = receiverPosition.distance(transitP).multiply(cReciprocal);
452 delta = FastMath.abs(delay.getReal() - previous);
453 } while (count++ < 10 && delta >= 2 * FastMath.ulp(delay.getReal()));
454
455 return delay;
456
457 }
458
459 /** Compute propagation delay on a link leg (typically downlink or uplink).
460 * @param emitterPosition fixed position of emitter
461 * @param emissionDate emission date
462 * @param adjustableReceiverPV position/velocity of emitter that may be adjusted
463 * @param approxReceptionDate approximate reception date
464 * @param frame inertial frame in which both {@code emitterPosition} and
465 * {@code adjustableReceiverPV} are defined
466 * @param <T> the type of the components
467 * @return <em>positive</em> delay between signal emission and signal reception dates
468 * @since 13.0
469 */
470 public static <T extends CalculusFieldElement<T>> T signalTimeOfFlightAdjustableReceiver(final FieldVector3D<T> emitterPosition,
471 final FieldAbsoluteDate<T> emissionDate,
472 final TimeStampedFieldPVCoordinates<T> adjustableReceiverPV,
473 final FieldAbsoluteDate<T> approxReceptionDate,
474 final Frame frame) {
475 return signalTimeOfFlightAdjustableReceiver(emitterPosition, emissionDate,
476 (FieldPVCoordinatesProvider<T>) new FieldAbsolutePVCoordinates<>(frame, adjustableReceiverPV),
477 approxReceptionDate, frame);
478 }
479
480 /** Compute propagation delay on a link leg (typically downlink or uplink).
481 * @param emitterPosition fixed position of emitter
482 * @param emissionDate emission date
483 * @param adjustableReceiver provider for adjusting receiver position
484 * @param approxReceptionDate approximate reception date
485 * @param frame inertial frame in which emitter is defined
486 * @param <T> the type of the components
487 * @return <em>positive</em> delay between signal emission and signal reception dates
488 * @since 13.0
489 */
490 public static <T extends CalculusFieldElement<T>> T signalTimeOfFlightAdjustableReceiver(final FieldVector3D<T> emitterPosition,
491 final FieldAbsoluteDate<T> emissionDate,
492 final FieldPVCoordinatesProvider<T> adjustableReceiver,
493 final FieldAbsoluteDate<T> approxReceptionDate,
494 final Frame frame) {
495
496 // initialize reception date search loop assuming the state is already correct
497 final T offset = approxReceptionDate.durationFrom(emissionDate);
498 T delay = offset;
499
500 // search signal transit date, computing the signal travel in the frame shared by emitter and receiver
501 final double cReciprocal = 1.0 / Constants.SPEED_OF_LIGHT;
502 double delta;
503 int count = 0;
504 do {
505 final double previous = delay.getReal();
506 final FieldVector3D<T> arrivalP = adjustableReceiver.getPosition(approxReceptionDate.shiftedBy(delay.subtract(offset)),
507 frame);
508 delay = arrivalP.distance(emitterPosition).multiply(cReciprocal);
509 delta = FastMath.abs(delay.getReal() - previous);
510 } while (count++ < 10 && delta >= 2 * FastMath.ulp(delay.getReal()));
511
512 return delay;
513
514 }
515
516 /** Get Cartesian coordinates as derivatives.
517 * <p>
518 * The position will correspond to variables {@code firstDerivative},
519 * {@code firstDerivative + 1} and {@code firstDerivative + 2}.
520 * The velocity will correspond to variables {@code firstDerivative + 3},
521 * {@code firstDerivative + 4} and {@code firstDerivative + 5}.
522 * The acceleration will correspond to constants.
523 * </p>
524 * @param state state of the satellite considered
525 * @param firstDerivative index of the first derivative
526 * @param freeParameters total number of free parameters in the gradient
527 * @return Cartesian coordinates as derivatives
528 * @since 10.2
529 */
530 public static TimeStampedFieldPVCoordinates<Gradient> getCoordinates(final SpacecraftState state,
531 final int firstDerivative,
532 final int freeParameters) {
533
534 // Position of the satellite expressed as a gradient
535 // The components of the position are the 3 first derivative parameters
536 final Vector3D p = state.getPosition();
537 final FieldVector3D<Gradient> pDS =
538 new FieldVector3D<>(Gradient.variable(freeParameters, firstDerivative, p.getX()),
539 Gradient.variable(freeParameters, firstDerivative + 1, p.getY()),
540 Gradient.variable(freeParameters, firstDerivative + 2, p.getZ()));
541
542 // Velocity of the satellite expressed as a gradient
543 // The components of the velocity are the 3 second derivative parameters
544 final Vector3D v = state.getVelocity();
545 final FieldVector3D<Gradient> vDS =
546 new FieldVector3D<>(Gradient.variable(freeParameters, firstDerivative + 3, v.getX()),
547 Gradient.variable(freeParameters, firstDerivative + 4, v.getY()),
548 Gradient.variable(freeParameters, firstDerivative + 5, v.getZ()));
549
550 // Acceleration of the satellite
551 // The components of the acceleration are not derivative parameters
552 final Vector3D a = state.getPVCoordinates().getAcceleration();
553 final FieldVector3D<Gradient> aDS =
554 new FieldVector3D<>(Gradient.constant(freeParameters, a.getX()),
555 Gradient.constant(freeParameters, a.getY()),
556 Gradient.constant(freeParameters, a.getZ()));
557
558 return new TimeStampedFieldPVCoordinates<>(state.getDate(), pDS, vDS, aDS);
559
560 }
561
562 }