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