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.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.propagation.SpacecraftState;
29 import org.orekit.time.AbsoluteDate;
30 import org.orekit.time.FieldAbsoluteDate;
31 import org.orekit.utils.Constants;
32 import org.orekit.utils.ParameterDriver;
33 import org.orekit.utils.TimeStampedFieldPVCoordinates;
34 import org.orekit.utils.TimeStampedPVCoordinates;
35
36 /** Abstract class handling measurements boilerplate.
37 * @param <T> the type of the measurement
38 * @author Luc Maisonobe
39 * @since 8.0
40 */
41 public abstract class AbstractMeasurement<T extends ObservedMeasurement<T>>
42 implements ObservedMeasurement<T> {
43
44 /** List of the supported parameters. */
45 private final List<ParameterDriver> supportedParameters;
46
47 /** Satellites related to this measurement.
48 * @since 9.3
49 */
50 private final List<ObservableSatellite> satellites;
51
52 /** Date of the measurement. */
53 private final AbsoluteDate date;
54
55 /** Observed value. */
56 private final double[] observed;
57
58 /** Theoretical standard deviation. */
59 private final double[] sigma;
60
61 /** Base weight. */
62 private final double[] baseWeight;
63
64 /** Modifiers that apply to the measurement.*/
65 private final List<EstimationModifier<T>> modifiers;
66
67 /** Enabling status. */
68 private boolean enabled;
69
70 /** Simple constructor for mono-dimensional measurements.
71 * <p>
72 * At construction, a measurement is enabled.
73 * </p>
74 * @param date date of the measurement
75 * @param observed observed value
76 * @param sigma theoretical standard deviation
77 * @param baseWeight base weight
78 * @param satellites satellites related to this measurement
79 * @since 9.3
80 */
81 protected AbstractMeasurement(final AbsoluteDate date, final double observed,
82 final double sigma, final double baseWeight,
83 final List<ObservableSatellite> satellites) {
84
85 this.supportedParameters = new ArrayList<ParameterDriver>();
86
87 this.date = date;
88 this.observed = new double[] {
89 observed
90 };
91 this.sigma = new double[] {
92 sigma
93 };
94 this.baseWeight = new double[] {
95 baseWeight
96 };
97
98 this.satellites = satellites;
99
100 this.modifiers = new ArrayList<EstimationModifier<T>>();
101 setEnabled(true);
102
103 }
104
105 /** Simple constructor, for multi-dimensional measurements.
106 * <p>
107 * At construction, a measurement is enabled.
108 * </p>
109 * @param date date of the measurement
110 * @param observed observed value
111 * @param sigma theoretical standard deviation
112 * @param baseWeight base weight
113 * @param satellites satellites related to this measurement
114 * @since 9.3
115 */
116 protected AbstractMeasurement(final AbsoluteDate date, final double[] observed,
117 final double[] sigma, final double[] baseWeight,
118 final List<ObservableSatellite> satellites) {
119 this.supportedParameters = new ArrayList<ParameterDriver>();
120
121 this.date = date;
122 this.observed = observed.clone();
123 this.sigma = sigma.clone();
124 this.baseWeight = baseWeight.clone();
125
126 this.satellites = satellites;
127
128 this.modifiers = new ArrayList<EstimationModifier<T>>();
129 setEnabled(true);
130
131 }
132
133 /** Add a parameter driver.
134 * @param driver parameter driver to add
135 * @since 9.3
136 */
137 protected void addParameterDriver(final ParameterDriver driver) {
138 supportedParameters.add(driver);
139 }
140
141 /** {@inheritDoc} */
142 @Override
143 public List<ParameterDriver> getParametersDrivers() {
144 return Collections.unmodifiableList(supportedParameters);
145 }
146
147 /** {@inheritDoc} */
148 @Override
149 public void setEnabled(final boolean enabled) {
150 this.enabled = enabled;
151 }
152
153 /** {@inheritDoc} */
154 @Override
155 public boolean isEnabled() {
156 return enabled;
157 }
158
159 /** {@inheritDoc} */
160 @Override
161 public int getDimension() {
162 return observed.length;
163 }
164
165 /** {@inheritDoc} */
166 @Override
167 public double[] getTheoreticalStandardDeviation() {
168 return sigma.clone();
169 }
170
171 /** {@inheritDoc} */
172 @Override
173 public double[] getBaseWeight() {
174 return baseWeight.clone();
175 }
176
177 /** {@inheritDoc} */
178 @Override
179 public List<ObservableSatellite> getSatellites() {
180 return satellites;
181 }
182
183 /** Estimate the theoretical value.
184 * <p>
185 * The theoretical value does not have <em>any</em> modifiers applied.
186 * </p>
187 * @param iteration iteration number
188 * @param evaluation evaluation number
189 * @param states orbital states at measurement date
190 * @return theoretical value
191 * @see #estimate(int, int, SpacecraftState[])
192 */
193 protected abstract EstimatedMeasurement<T> theoreticalEvaluation(int iteration, int evaluation, SpacecraftState[] states);
194
195 /** {@inheritDoc} */
196 @Override
197 public EstimatedMeasurement<T> estimate(final int iteration, final int evaluation, final SpacecraftState[] states) {
198
199 // compute the theoretical value
200 final EstimatedMeasurement<T> estimation = theoreticalEvaluation(iteration, evaluation, states);
201
202 // apply the modifiers
203 for (final EstimationModifier<T> modifier : modifiers) {
204 modifier.modify(estimation);
205 }
206
207 return estimation;
208
209 }
210
211 /** {@inheritDoc} */
212 @Override
213 public AbsoluteDate getDate() {
214 return date;
215 }
216
217 /** {@inheritDoc} */
218 @Override
219 public double[] getObservedValue() {
220 return observed.clone();
221 }
222
223 /** {@inheritDoc} */
224 @Override
225 public void addModifier(final EstimationModifier<T> modifier) {
226
227 // combine the measurement parameters and the modifier parameters
228 supportedParameters.addAll(modifier.getParametersDrivers());
229
230 modifiers.add(modifier);
231
232 }
233
234 /** {@inheritDoc} */
235 @Override
236 public List<EstimationModifier<T>> getModifiers() {
237 return Collections.unmodifiableList(modifiers);
238 }
239
240 /** Compute propagation delay on a link leg (typically downlink or uplink).
241 * @param adjustableEmitterPV position/velocity of emitter that may be adjusted
242 * @param receiverPosition fixed position of receiver at {@code signalArrivalDate},
243 * in the same frame as {@code adjustableEmitterPV}
244 * @param signalArrivalDate date at which the signal arrives to receiver
245 * @return <em>positive</em> delay between signal emission and signal reception dates
246 */
247 public static double signalTimeOfFlight(final TimeStampedPVCoordinates adjustableEmitterPV,
248 final Vector3D receiverPosition,
249 final AbsoluteDate signalArrivalDate) {
250
251 // initialize emission date search loop assuming the state is already correct
252 // this will be true for all but the first orbit determination iteration,
253 // and even for the first iteration the loop will converge very fast
254 final double offset = signalArrivalDate.durationFrom(adjustableEmitterPV.getDate());
255 double delay = offset;
256
257 // search signal transit date, computing the signal travel in inertial frame
258 final double cReciprocal = 1.0 / Constants.SPEED_OF_LIGHT;
259 double delta;
260 int count = 0;
261 do {
262 final double previous = delay;
263 final Vector3D transitP = adjustableEmitterPV.shiftedBy(offset - delay).getPosition();
264 delay = receiverPosition.distance(transitP) * cReciprocal;
265 delta = FastMath.abs(delay - previous);
266 } while (count++ < 10 && delta >= 2 * FastMath.ulp(delay));
267
268 return delay;
269
270 }
271
272 /** Compute propagation delay on a link leg (typically downlink or uplink).
273 * @param adjustableEmitterPV position/velocity of emitter that may be adjusted
274 * @param receiverPosition fixed position of receiver at {@code signalArrivalDate},
275 * in the same frame as {@code adjustableEmitterPV}
276 * @param signalArrivalDate date at which the signal arrives to receiver
277 * @return <em>positive</em> delay between signal emission and signal reception dates
278 * @param <T> the type of the components
279 */
280 public static <T extends CalculusFieldElement<T>> T signalTimeOfFlight(final TimeStampedFieldPVCoordinates<T> adjustableEmitterPV,
281 final FieldVector3D<T> receiverPosition,
282 final FieldAbsoluteDate<T> signalArrivalDate) {
283
284 // Initialize emission date search loop assuming the emitter PV is almost correct
285 // this will be true for all but the first orbit determination iteration,
286 // and even for the first iteration the loop will converge extremely fast
287 final T offset = signalArrivalDate.durationFrom(adjustableEmitterPV.getDate());
288 T delay = offset;
289
290 // search signal transit date, computing the signal travel in the frame shared by emitter and receiver
291 final double cReciprocal = 1.0 / Constants.SPEED_OF_LIGHT;
292 double delta;
293 int count = 0;
294 do {
295 final double previous = delay.getReal();
296 final FieldVector3D<T> transitP = adjustableEmitterPV.shiftedBy(delay.negate().add(offset)).getPosition();
297 delay = receiverPosition.distance(transitP).multiply(cReciprocal);
298 delta = FastMath.abs(delay.getReal() - previous);
299 } while (count++ < 10 && delta >= 2 * FastMath.ulp(delay.getReal()));
300
301 return delay;
302
303 }
304
305 /** Get Cartesian coordinates as derivatives.
306 * <p>
307 * The position will correspond to variables {@code firstDerivative},
308 * {@code firstDerivative + 1} and {@code firstDerivative + 2}.
309 * The velocity will correspond to variables {@code firstDerivative + 3},
310 * {@code firstDerivative + 4} and {@code firstDerivative + 5}.
311 * The acceleration will correspond to constants.
312 * </p>
313 * @param state state of the satellite considered
314 * @param firstDerivative index of the first derivative
315 * @param freeParameters total number of free parameters in the gradient
316 * @return Cartesian coordinates as derivatives
317 * @since 10.2
318 */
319 public static TimeStampedFieldPVCoordinates<Gradient> getCoordinates(final SpacecraftState state,
320 final int firstDerivative,
321 final int freeParameters) {
322
323 // Position of the satellite expressed as a gradient
324 // The components of the position are the 3 first derivative parameters
325 final Vector3D p = state.getPVCoordinates().getPosition();
326 final FieldVector3D<Gradient> pDS =
327 new FieldVector3D<>(Gradient.variable(freeParameters, firstDerivative + 0, p.getX()),
328 Gradient.variable(freeParameters, firstDerivative + 1, p.getY()),
329 Gradient.variable(freeParameters, firstDerivative + 2, p.getZ()));
330
331 // Velocity of the satellite expressed as a gradient
332 // The components of the velocity are the 3 second derivative parameters
333 final Vector3D v = state.getPVCoordinates().getVelocity();
334 final FieldVector3D<Gradient> vDS =
335 new FieldVector3D<>(Gradient.variable(freeParameters, firstDerivative + 3, v.getX()),
336 Gradient.variable(freeParameters, firstDerivative + 4, v.getY()),
337 Gradient.variable(freeParameters, firstDerivative + 5, v.getZ()));
338
339 // Acceleration of the satellite
340 // The components of the acceleration are not derivative parameters
341 final Vector3D a = state.getPVCoordinates().getAcceleration();
342 final FieldVector3D<Gradient> aDS =
343 new FieldVector3D<>(Gradient.constant(freeParameters, a.getX()),
344 Gradient.constant(freeParameters, a.getY()),
345 Gradient.constant(freeParameters, a.getZ()));
346
347 return new TimeStampedFieldPVCoordinates<>(state.getDate(), pDS, vDS, aDS);
348
349 }
350
351 }