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.Arrays;
20  import java.util.List;
21  
22  import org.hipparchus.stat.descriptive.StreamingStatistics;
23  import org.hipparchus.util.FastMath;
24  import org.junit.jupiter.api.Assertions;
25  import org.junit.jupiter.api.Test;
26  import org.orekit.estimation.Context;
27  import org.orekit.estimation.EstimationTestUtils;
28  import org.orekit.estimation.measurements.modifiers.TDOATroposphericDelayModifier;
29  import org.orekit.models.earth.troposphere.ModifiedSaastamoinenModel;
30  import org.orekit.orbits.OrbitType;
31  import org.orekit.orbits.PositionAngleType;
32  import org.orekit.propagation.Propagator;
33  import org.orekit.propagation.SpacecraftState;
34  import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
35  import org.orekit.time.AbsoluteDate;
36  import org.orekit.utils.Differentiation;
37  import org.orekit.utils.ParameterDriver;
38  import org.orekit.utils.ParameterFunction;
39  
40  public class TDOATest {
41  
42      /**
43       * Compare observed values and estimated values.
44       * Both are calculated with a different algorithm.
45       */
46      @Test
47      public void testValues() {
48  
49          Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
50  
51          // Create perfect measurements
52          final NumericalPropagatorBuilder propagatorBuilder =
53                          context.createBuilder(OrbitType.EQUINOCTIAL, PositionAngleType.TRUE, false,
54                                                1.0e-6, 60.0, 0.001);
55          final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
56                                                                             propagatorBuilder);
57          final List<ObservedMeasurement<?>> measurements =
58                          EstimationTestUtils.createMeasurements(propagator,
59                                                                 new TDOAMeasurementCreator(context),
60                                                                 1.0, 3.0, 300.0);
61          propagator.clearStepHandlers();
62  
63          // Prepare statistics for values difference
64          final StreamingStatistics diffStat = new StreamingStatistics();
65  
66          for (final ObservedMeasurement<?> measurement : measurements) {
67  
68              // Propagate to measurement date
69              SpacecraftState state = propagator.propagate(measurement.getDate());
70  
71              // Estimate the measurement value
72              final EstimatedMeasurementBase<?> estimated = measurement.estimateWithoutDerivatives(new SpacecraftState[] { state });
73  
74              // Store the difference between estimated and observed values in the stats
75              diffStat.addValue(FastMath.abs(estimated.getEstimatedValue()[0] - measurement.getObservedValue()[0]));
76  
77          }
78  
79          // Mean and std errors check
80          Assertions.assertEquals(0.0, diffStat.getMean(), 1.4e-16);
81          Assertions.assertEquals(0.0, diffStat.getStandardDeviation(), 2.0e-16);
82  
83          // Test measurement type
84          Assertions.assertEquals(TDOA.MEASUREMENT_TYPE, measurements.get(0).getMeasurementType());
85      }
86  
87      /**
88       * Test the values of the state derivatives using a numerical
89       * finite differences calculation as a reference.
90       */
91      @Test
92      public void testStateDerivatives() {
93  
94          Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
95  
96          // create perfect measurements
97          final NumericalPropagatorBuilder propagatorBuilder =
98                          context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
99                                                1.0e-6, 60.0, 0.001);
100         final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
101                                                                            propagatorBuilder);
102         final List<ObservedMeasurement<?>> measurements =
103                         EstimationTestUtils.createMeasurements(propagator,
104                                                                new TDOAMeasurementCreator(context),
105                                                                1.0, 3.0, 300.0);
106         propagator.clearStepHandlers();
107 
108         double maxRelativeError = 0;
109         for (final ObservedMeasurement<?> measurement : measurements) {
110 
111             final AbsoluteDate    date  = measurement.getDate().shiftedBy(1);
112             final SpacecraftState state = propagator.propagate(date);
113 
114             final EstimatedMeasurement<?> estimated = measurement.estimate(0, 0, new SpacecraftState[] { state });
115             Assertions.assertEquals(3, estimated.getParticipants().length);
116             final double[][] jacobian = estimated.getStateDerivatives(0);
117 
118             final double[][] finiteDifferencesJacobian =
119                     Differentiation.differentiate(state1 ->
120                                                       measurement.
121                                                           estimateWithoutDerivatives(new SpacecraftState[] { state1 }).
122                                                           getEstimatedValue(),
123                                                   1, propagator.getAttitudeProvider(),
124                                                   OrbitType.CARTESIAN, PositionAngleType.TRUE, 15.0, 3).value(state);
125 
126             Assertions.assertEquals(finiteDifferencesJacobian.length, jacobian.length);
127             Assertions.assertEquals(finiteDifferencesJacobian[0].length, jacobian[0].length);
128 
129             for (int i = 0; i < jacobian.length; ++i) {
130                 for (int j = 0; j < jacobian[i].length; ++j) {
131                     // check the values returned by getStateDerivatives() are correct
132                     maxRelativeError = FastMath.max(maxRelativeError,
133                                                     FastMath.abs((finiteDifferencesJacobian[i][j] - jacobian[i][j]) /
134                                                                   finiteDifferencesJacobian[i][j]));
135                 }
136             }
137         }
138 
139         Assertions.assertEquals(0, maxRelativeError, 4.7e-4);
140 
141     }
142 
143     /**
144      * Test the values of the state derivatives, using a numerical
145      * finite differences calculation as a reference, with modifiers (tropospheric corrections).
146      */
147     @Test
148     public void testStateDerivativesWithModifier() {
149 
150         Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
151 
152         // create perfect measurements
153         final NumericalPropagatorBuilder propagatorBuilder =
154                         context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
155                                               1.0e-6, 60.0, 0.001);
156         final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
157                                                                            propagatorBuilder);
158         final double clockOffset = 4.8e-9;
159         for (final GroundStation station : Arrays.asList(context.TDOAstations.getKey(),
160                                                          context.TDOAstations.getValue())) {
161             station.getClockOffsetDriver().setValue(clockOffset);
162         }
163         final List<ObservedMeasurement<?>> measurements =
164                         EstimationTestUtils.createMeasurements(propagator,
165                                                                new TDOAMeasurementCreator(context),
166                                                                1.0, 3.0, 300.0);
167         propagator.clearStepHandlers();
168 
169         // create a modifier
170         final TDOATroposphericDelayModifier modifier =
171                         new TDOATroposphericDelayModifier(ModifiedSaastamoinenModel.getStandardModel());
172 
173         double maxRelativeError = 0;
174         for (final ObservedMeasurement<?> measurement : measurements) {
175 
176             ((TDOA) measurement).addModifier(modifier);
177 
178             final AbsoluteDate    date  = measurement.getDate().shiftedBy(1);
179             final SpacecraftState state = propagator.propagate(date);
180 
181             final double[][] jacobian = measurement.estimate(0, 0, new SpacecraftState[] { state }).getStateDerivatives(0);
182 
183             final double[][] finiteDifferencesJacobian =
184                     Differentiation.differentiate(state1 ->
185                                                   measurement.
186                                                       estimateWithoutDerivatives(new SpacecraftState[] { state1 }).
187                                                       getEstimatedValue(),
188                                                   1, propagator.getAttitudeProvider(),
189                OrbitType.CARTESIAN, PositionAngleType.TRUE, 15.0, 3).value(state);
190 
191             Assertions.assertEquals(finiteDifferencesJacobian.length, jacobian.length);
192             Assertions.assertEquals(finiteDifferencesJacobian[0].length, jacobian[0].length);
193 
194             for (int i = 0; i < jacobian.length; ++i) {
195                 for (int j = 0; j < jacobian[i].length; ++j) {
196                     // check the values returned by getStateDerivatives() are correct
197                     maxRelativeError = FastMath.max(maxRelativeError,
198                                                     FastMath.abs((finiteDifferencesJacobian[i][j] - jacobian[i][j]) /
199                                                                  finiteDifferencesJacobian[i][j]));
200                 }
201             }
202 
203         }
204 
205         Assertions.assertEquals(0, maxRelativeError, 3.2e-3);
206 
207     }
208 
209     /**
210      * Test the values of the parameters' derivatives using a numerical
211      * finite differences calculation as a reference.
212      */
213     @Test
214     public void testParameterDerivatives() {
215 
216         Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
217 
218         // create perfect measurements
219         final NumericalPropagatorBuilder propagatorBuilder =
220                         context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
221                                               1.0e-6, 60.0, 0.001);
222         final TDOAMeasurementCreator creator = new TDOAMeasurementCreator(context);
223         final GroundStation primary = context.TDOAstations.getKey();
224         primary.getEastOffsetDriver().setSelected(true);
225         primary.getNorthOffsetDriver().setSelected(true);
226         primary.getZenithOffsetDriver().setSelected(true);
227         final double clockOffset = 4.8e-9;
228         final GroundStation secondary = context.TDOAstations.getValue();
229         secondary.getClockOffsetDriver().setValue(clockOffset);
230         secondary.getClockOffsetDriver().setSelected(true);
231         secondary.getEastOffsetDriver().setSelected(true);
232         secondary.getNorthOffsetDriver().setSelected(true);
233         secondary.getZenithOffsetDriver().setSelected(true);
234         final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
235                                                                            propagatorBuilder);
236         final List<ObservedMeasurement<?>> measurements =
237                         EstimationTestUtils.createMeasurements(propagator,
238                                                                creator,
239                                                                1.0, 3.0, 300.0);
240         propagator.clearStepHandlers();
241 
242         double maxRelativeError = 0;
243         for (final ObservedMeasurement<?> measurement : measurements) {
244 
245             // parameter corresponding to station position offset
246             final GroundStation primeParameter  = ((TDOA) measurement).getPrimeStation();
247             final GroundStation secondParameter = ((TDOA) measurement).getSecondStation();
248 
249             // We intentionally propagate to a date which is close to the
250             // real spacecraft state but is *not* the accurate date, by
251             // compensating only part of the downlink delay. This is done
252             // in order to validate the partial derivatives with respect
253             // to velocity. If we had chosen the proper state date, the
254             // range would have depended only on the current position but
255             // not on the current velocity.
256             final double          delay = measurement.getObservedValue()[0];
257             final AbsoluteDate    date  = measurement.getDate().shiftedBy(delay);
258             final SpacecraftState state = propagator.propagate(date);
259             final ParameterDriver[] drivers = new ParameterDriver[] {
260                 primeParameter.getEastOffsetDriver(),
261                 primeParameter.getNorthOffsetDriver(),
262                 primeParameter.getZenithOffsetDriver(),
263                 secondParameter.getClockOffsetDriver(),
264                 secondParameter.getEastOffsetDriver(),
265                 secondParameter.getNorthOffsetDriver(),
266                 secondParameter.getZenithOffsetDriver(),
267             };
268             for (int i = 0; i < drivers.length; ++i) {
269                 final double[] gradient = measurement.estimate(0, 0, new SpacecraftState[] { state }).getParameterDerivatives(drivers[i], new AbsoluteDate());
270                 Assertions.assertEquals(1, measurement.getDimension());
271                 Assertions.assertEquals(1, gradient.length);
272 
273                 final ParameterFunction dMkdP =
274                                 Differentiation.differentiate(new ParameterFunction() {
275                                     /** {@inheritDoc} */
276                                     @Override
277                                     public double value(final ParameterDriver parameterDriver, AbsoluteDate date) {
278                                         return measurement.
279                                                estimateWithoutDerivatives(new SpacecraftState[] { state }).
280                                                getEstimatedValue()[0];
281                                     }
282                                 }, 3, 20.0 * drivers[i].getScale());
283                 final double ref = dMkdP.value(drivers[i], date);
284                 maxRelativeError = FastMath.max(maxRelativeError, FastMath.abs((ref - gradient[0]) / ref));
285             }
286         }
287 
288         Assertions.assertEquals(0, maxRelativeError, 9.0e-7);
289 
290     }
291 
292     /**
293      * Test the values of the parameters' derivatives, using a numerical
294      * finite differences calculation as a reference, with modifiers (tropospheric corrections).
295      */
296     @Test
297     public void testParameterDerivativesWithModifier() {
298 
299         Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
300 
301         // create perfect measurements
302         final NumericalPropagatorBuilder propagatorBuilder =
303                         context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
304                                               1.0e-6, 60.0, 0.001);
305         final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
306                                                                            propagatorBuilder);
307         final GroundStation primary = context.TDOAstations.getKey();
308         primary.getEastOffsetDriver().setSelected(true);
309         primary.getNorthOffsetDriver().setSelected(true);
310         primary.getZenithOffsetDriver().setSelected(true);
311         final double clockOffset = 4.8e-9;
312         final GroundStation secondary = context.TDOAstations.getValue();
313         secondary.getClockOffsetDriver().setValue(clockOffset);
314         secondary.getClockOffsetDriver().setSelected(true);
315         secondary.getEastOffsetDriver().setSelected(true);
316         secondary.getNorthOffsetDriver().setSelected(true);
317         secondary.getZenithOffsetDriver().setSelected(true);
318         final TDOAMeasurementCreator creator = new TDOAMeasurementCreator(context);
319         final List<ObservedMeasurement<?>> measurements =
320                         EstimationTestUtils.createMeasurements(propagator,
321                                                                creator,
322                                                                1.0, 3.0, 300.0);
323         propagator.clearStepHandlers();
324 
325         // create a modifier
326         final TDOATroposphericDelayModifier modifier =
327                         new TDOATroposphericDelayModifier(ModifiedSaastamoinenModel.getStandardModel());
328 
329         double maxRelativeError = 0;
330         for (final ObservedMeasurement<?> measurement : measurements) {
331 
332             ((TDOA) measurement).addModifier(modifier);
333 
334             // parameter corresponding to station position offset
335             final GroundStation primeParameter  = ((TDOA) measurement).getPrimeStation();
336             final GroundStation secondParameter = ((TDOA) measurement).getSecondStation();
337 
338             // We intentionally propagate to a date which is close to the
339             // real spacecraft state but is *not* the accurate date, by
340             // compensating only part of the downlink delay. This is done
341             // in order to validate the partial derivatives with respect
342             // to velocity. If we had chosen the proper state date, the
343             // range would have depended only on the current position but
344             // not on the current velocity.
345             final double          delay = measurement.getObservedValue()[0];
346             final AbsoluteDate    date  = measurement.getDate().shiftedBy(delay);
347             final SpacecraftState state = propagator.propagate(date);
348             final ParameterDriver[] drivers = new ParameterDriver[] {
349                 primeParameter.getEastOffsetDriver(),
350                 primeParameter.getNorthOffsetDriver(),
351                 primeParameter.getZenithOffsetDriver(),
352                 secondParameter.getClockOffsetDriver(),
353                 secondParameter.getEastOffsetDriver(),
354                 secondParameter.getNorthOffsetDriver(),
355                 secondParameter.getZenithOffsetDriver(),
356             };
357             for (int i = 0; i < drivers.length; ++i) {
358                 final double[] gradient = measurement.estimate(0, 0, new SpacecraftState[] { state }).getParameterDerivatives(drivers[i], new AbsoluteDate());
359                 Assertions.assertEquals(1, measurement.getDimension());
360                 Assertions.assertEquals(1, gradient.length);
361 
362                 final ParameterFunction dMkdP =
363                                 Differentiation.differentiate(new ParameterFunction() {
364                                     /** {@inheritDoc} */
365                                     @Override
366                                     public double value(final ParameterDriver parameterDriver, AbsoluteDate date) {
367                                         return measurement.
368                                                estimateWithoutDerivatives(new SpacecraftState[] { state }).
369                                                getEstimatedValue()[0];
370                                     }
371                                 }, 3, 20.0 * drivers[i].getScale());
372                 final double ref = dMkdP.value(drivers[i], date);
373                 maxRelativeError = FastMath.max(maxRelativeError, FastMath.abs((ref - gradient[0]) / ref));
374             }
375         }
376 
377         Assertions.assertEquals(0, maxRelativeError, 2.5e-6);
378 
379     }
380 
381 }