1   /* Copyright 2002-2020 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.List;
20  
21  import org.hipparchus.util.FastMath;
22  import org.junit.Assert;
23  import org.junit.Test;
24  import org.orekit.estimation.DSSTContext;
25  import org.orekit.estimation.DSSTEstimationTestUtils;
26  import org.orekit.estimation.EstimationTestUtils;
27  import org.orekit.estimation.measurements.modifiers.RangeRateTroposphericDelayModifier;
28  import org.orekit.models.earth.troposphere.SaastamoinenModel;
29  import org.orekit.orbits.OrbitType;
30  import org.orekit.orbits.PositionAngle;
31  import org.orekit.propagation.Propagator;
32  import org.orekit.propagation.SpacecraftState;
33  import org.orekit.propagation.conversion.DSSTPropagatorBuilder;
34  import org.orekit.time.AbsoluteDate;
35  import org.orekit.utils.Constants;
36  import org.orekit.utils.Differentiation;
37  import org.orekit.utils.ParameterDriver;
38  import org.orekit.utils.ParameterFunction;
39  import org.orekit.utils.StateFunction;
40  
41  public class DSSTRangeRateTest {
42  
43      @Test
44      public void testStateDerivativesOneWay() {
45  
46          DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
47  
48          final DSSTPropagatorBuilder propagatorBuilder =
49                          context.createBuilder(true, 1.0e-6, 60.0, 0.001);
50  
51          // create perfect range rate measurements
52          final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit,
53                                                                             propagatorBuilder);
54  
55          final double groundClockDrift =  4.8e-9;
56          for (final GroundStation station : context.stations) {
57              station.getClockDriftDriver().setValue(groundClockDrift);
58          }
59          final double satClkDrift = 3.2e-10;
60          final List<ObservedMeasurement<?>> measurements =
61                          DSSTEstimationTestUtils.createMeasurements(propagator,
62                                                                 new DSSTRangeRateMeasurementCreator(context, false, satClkDrift),
63                                                                 1.0, 3.0, 300.0);
64          for (final ObservedMeasurement<?> m : measurements) {
65              Assert.assertFalse(((RangeRate) m).isTwoWay());
66          }
67          propagator.setSlaveMode();
68  
69          double maxRelativeError = 0;
70          for (final ObservedMeasurement<?> measurement : measurements) {
71  
72              final double          meanDelay = 1; // measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
73              final AbsoluteDate    date      = measurement.getDate().shiftedBy(-0.75 * meanDelay);
74              final SpacecraftState state = propagator.propagate(date);
75  
76              final EstimatedMeasurement<?> estimated = measurement.estimate(0, 0, new SpacecraftState[] { state });
77              Assert.assertEquals(2, estimated.getParticipants().length);
78              final double[][] jacobian = estimated.getStateDerivatives(0);
79  
80              final double[][] finiteDifferencesJacobian =
81                      Differentiation.differentiate(new StateFunction() {
82                  public double[] value(final SpacecraftState state) {
83                      return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue();
84                  }
85              }, 1, propagator.getAttitudeProvider(),
86                 OrbitType.CARTESIAN, PositionAngle.TRUE, 15.0, 3).value(state);
87  
88              Assert.assertEquals(finiteDifferencesJacobian.length, jacobian.length);
89              Assert.assertEquals(finiteDifferencesJacobian[0].length, jacobian[0].length);
90  
91              for (int i = 0; i < jacobian.length; ++i) {
92                  for (int j = 0; j < jacobian[i].length; ++j) {
93                      // check the values returned by getStateDerivatives() are correct
94                      maxRelativeError = FastMath.max(maxRelativeError,
95                                                      FastMath.abs((finiteDifferencesJacobian[i][j] - jacobian[i][j]) /
96                                                                   finiteDifferencesJacobian[i][j]));
97                  }
98              }
99  
100         }
101         Assert.assertEquals(0, maxRelativeError, 4.7e-9);
102 
103     }
104 
105 
106     @Test
107     public void testStateDerivativesTwoWays() {
108 
109         DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
110 
111         final DSSTPropagatorBuilder propagatorBuilder =
112                         context.createBuilder(true, 1.0e-6, 60.0, 0.001);
113 
114         // create perfect range rate measurements
115         final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit,
116                                                                            propagatorBuilder);
117         final double groundClockDrift =  4.8e-9;
118         for (final GroundStation station : context.stations) {
119             station.getClockDriftDriver().setValue(groundClockDrift);
120         }
121         final double satClkDrift = 3.2e-10;
122         final List<ObservedMeasurement<?>> measurements =
123                         DSSTEstimationTestUtils.createMeasurements(propagator,
124                                                                new DSSTRangeRateMeasurementCreator(context, true, satClkDrift),
125                                                                1.0, 3.0, 300.0);
126         for (final ObservedMeasurement<?> m : measurements) {
127             Assert.assertTrue(((RangeRate) m).isTwoWay());
128         }
129         propagator.setSlaveMode();
130 
131         double maxRelativeError = 0;
132         for (final ObservedMeasurement<?> measurement : measurements) {
133 
134             //
135             //final AbsoluteDate date = measurement.getDate();
136             final double          meanDelay = 1; // measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
137             final AbsoluteDate    date      = measurement.getDate().shiftedBy(-0.75 * meanDelay);
138             final SpacecraftState state     = propagator.propagate(date);
139 
140             final EstimatedMeasurement<?> estimated = measurement.estimate(0, 0, new SpacecraftState[] { state });
141             Assert.assertEquals(3, estimated.getParticipants().length);
142             final double[][] jacobian = estimated.getStateDerivatives(0);
143 
144             final double[][] finiteDifferencesJacobian =
145                     Differentiation.differentiate(new StateFunction() {
146                 public double[] value(final SpacecraftState state) {
147                     return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue();
148                 }
149             }, 1, propagator.getAttitudeProvider(),
150                OrbitType.CARTESIAN, PositionAngle.TRUE, 15.0, 3).value(state);
151 
152             Assert.assertEquals(finiteDifferencesJacobian.length, jacobian.length);
153             Assert.assertEquals(finiteDifferencesJacobian[0].length, jacobian[0].length);
154 
155             for (int i = 0; i < jacobian.length; ++i) {
156                 for (int j = 0; j < jacobian[i].length; ++j) {
157                     // check the values returned by getStateDerivatives() are correct
158                     maxRelativeError = FastMath.max(maxRelativeError,
159                                                     FastMath.abs((finiteDifferencesJacobian[i][j] - jacobian[i][j]) /
160                                                                  finiteDifferencesJacobian[i][j]));
161                 }
162             }
163 
164         }
165         Assert.assertEquals(0, maxRelativeError, 1.1e-7);
166 
167     }
168 
169     @Test
170     public void testParameterDerivativesOneWay() {
171 
172         DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
173 
174         final DSSTPropagatorBuilder propagatorBuilder =
175                         context.createBuilder(true, 1.0e-6, 60.0, 0.001);
176 
177         // create perfect range rate measurements
178         final double groundClockDrift =  4.8e-9;
179         for (final GroundStation station : context.stations) {
180             station.getClockDriftDriver().setValue(groundClockDrift);
181         }
182         final double satClkDrift = 3.2e-10;
183         final DSSTRangeRateMeasurementCreatorementCreator.html#DSSTRangeRateMeasurementCreator">DSSTRangeRateMeasurementCreator creator = new DSSTRangeRateMeasurementCreator(context, false, satClkDrift);
184         creator.getSatellite().getClockDriftDriver().setSelected(true);
185         for (final GroundStation station : context.stations) {
186             station.getClockDriftDriver().setSelected(true);
187             station.getEastOffsetDriver().setSelected(true);
188             station.getNorthOffsetDriver().setSelected(true);
189             station.getZenithOffsetDriver().setSelected(true);
190         }
191         final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit,
192                                                                            propagatorBuilder);
193         final List<ObservedMeasurement<?>> measurements =
194                         EstimationTestUtils.createMeasurements(propagator,
195                                                                creator,
196                                                                1.0, 3.0, 300.0);
197         propagator.setSlaveMode();
198 
199         double maxRelativeError = 0;
200         for (final ObservedMeasurement<?> measurement : measurements) {
201 
202             // parameter corresponding to station position offset
203             final GroundStation stationParameter = ((RangeRate) measurement).getStation();
204 
205             // We intentionally propagate to a date which is close to the
206             // real spacecraft state but is *not* the accurate date, by
207             // compensating only part of the downlink delay. This is done
208             // in order to validate the partial derivatives with respect
209             // to velocity. If we had chosen the proper state date, the
210             // range would have depended only on the current position but
211             // not on the current velocity.
212             final double          meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
213             final AbsoluteDate    date      = measurement.getDate().shiftedBy(-0.75 * meanDelay);
214             final SpacecraftState state     = propagator.propagate(date);
215             final ParameterDriver[] drivers = new ParameterDriver[] {
216                 stationParameter.getClockDriftDriver(),
217                 stationParameter.getEastOffsetDriver(),
218                 stationParameter.getNorthOffsetDriver(),
219                 stationParameter.getZenithOffsetDriver(),
220                 measurement.getSatellites().get(0).getClockDriftDriver()
221             };
222             for (int i = 0; i < drivers.length; ++i) {
223                 final double[] gradient  = measurement.estimate(0, 0, new SpacecraftState[] { state }).getParameterDerivatives(drivers[i]);
224                 Assert.assertEquals(1, measurement.getDimension());
225                 Assert.assertEquals(1, gradient.length);
226 
227                 final ParameterFunction dMkdP =
228                                 Differentiation.differentiate(new ParameterFunction() {
229                                     /** {@inheritDoc} */
230                                     @Override
231                                     public double value(final ParameterDriver parameterDriver) {
232                                         return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue()[0];
233                                     }
234                                 }, 3, 20.0 * drivers[i].getScale());
235                 final double ref = dMkdP.value(drivers[i]);
236                 maxRelativeError = FastMath.max(maxRelativeError, FastMath.abs((ref - gradient[0]) / ref));
237             }
238 
239         }
240         Assert.assertEquals(0, maxRelativeError, 8.3e-7);
241 
242     }
243 
244     @Test
245     public void testParameterDerivativesTwoWays() {
246 
247         DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
248 
249         final DSSTPropagatorBuilder propagatorBuilder =
250                         context.createBuilder(true, 1.0e-6, 60.0, 0.001);
251 
252         // create perfect range rate measurements
253         final double groundClockDrift =  4.8e-9;
254         for (final GroundStation station : context.stations) {
255             station.getClockDriftDriver().setValue(groundClockDrift);
256         }
257         final double satClkDrift = 3.2e-10;
258         final DSSTRangeRateMeasurementCreatorementCreator.html#DSSTRangeRateMeasurementCreator">DSSTRangeRateMeasurementCreator creator = new DSSTRangeRateMeasurementCreator(context, true, satClkDrift);
259         for (final GroundStation station : context.stations) {
260             station.getEastOffsetDriver().setSelected(true);
261             station.getNorthOffsetDriver().setSelected(true);
262             station.getZenithOffsetDriver().setSelected(true);
263         }
264         final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit,
265                                                                            propagatorBuilder);
266         final List<ObservedMeasurement<?>> measurements =
267                         DSSTEstimationTestUtils.createMeasurements(propagator,
268                                                                creator,
269                                                                1.0, 3.0, 300.0);
270         propagator.setSlaveMode();
271 
272         double maxRelativeError = 0;
273         for (final ObservedMeasurement<?> measurement : measurements) {
274 
275             // parameter corresponding to station position offset
276             final GroundStation stationParameter = ((RangeRate) measurement).getStation();
277 
278             // We intentionally propagate to a date which is close to the
279             // real spacecraft state but is *not* the accurate date, by
280             // compensating only part of the downlink delay. This is done
281             // in order to validate the partial derivatives with respect
282             // to velocity. If we had chosen the proper state date, the
283             // range would have depended only on the current position but
284             // not on the current velocity.
285             final double          meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
286             final AbsoluteDate    date      = measurement.getDate().shiftedBy(-0.75 * meanDelay);
287             final SpacecraftState state     = propagator.propagate(date);
288             final ParameterDriver[] drivers = new ParameterDriver[] {
289                 stationParameter.getEastOffsetDriver(),
290                 stationParameter.getNorthOffsetDriver(),
291                 stationParameter.getZenithOffsetDriver()
292             };
293             for (int i = 0; i < drivers.length; ++i) {
294                 final double[] gradient  = measurement.estimate(0, 0, new SpacecraftState[] { state }).getParameterDerivatives(drivers[i]);
295                 Assert.assertEquals(1, measurement.getDimension());
296                 Assert.assertEquals(1, gradient.length);
297 
298                 final ParameterFunction dMkdP =
299                                 Differentiation.differentiate(new ParameterFunction() {
300                                     /** {@inheritDoc} */
301                                     @Override
302                                     public double value(final ParameterDriver parameterDriver) {
303                                         return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue()[0];
304                                     }
305                                 }, 3, 20.0 * drivers[i].getScale());
306                 final double ref = dMkdP.value(drivers[i]);
307                 maxRelativeError = FastMath.max(maxRelativeError, FastMath.abs((ref - gradient[0]) / ref));
308             }
309 
310         }
311         Assert.assertEquals(0, maxRelativeError, 8.2e-7);
312 
313     }
314 
315     @Test
316     public void testStateDerivativesWithModifier() {
317 
318         DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
319 
320         final DSSTPropagatorBuilder propagatorBuilder =
321                         context.createBuilder(true, 1.0e-6, 60.0, 0.001);
322 
323         // create perfect range rate measurements
324         final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit,
325                                                                            propagatorBuilder);
326         final double groundClockDrift =  4.8e-9;
327         for (final GroundStation station : context.stations) {
328             station.getClockDriftDriver().setValue(groundClockDrift);
329         }
330         final double satClkDrift = 3.2e-10;
331         final DSSTRangeRateMeasurementCreatorementCreator.html#DSSTRangeRateMeasurementCreator">DSSTRangeRateMeasurementCreator creator = new DSSTRangeRateMeasurementCreator(context, false, satClkDrift);
332         final List<ObservedMeasurement<?>> measurements =
333                         DSSTEstimationTestUtils.createMeasurements(propagator,
334                                                                creator,
335                                                                1.0, 3.0, 300.0);
336         propagator.setSlaveMode();
337 
338         double maxRelativeError = 0;
339         for (final ObservedMeasurement<?> measurement : measurements) {
340 
341             final RangeRateTroposphericDelayModifier modifier = new RangeRateTroposphericDelayModifier(SaastamoinenModel.getStandardModel(), true);
342             ((RangeRate) measurement).addModifier(modifier);
343 
344             //
345             //final AbsoluteDate date = measurement.getDate();
346             final double          meanDelay = 1; // measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
347             final AbsoluteDate    date      = measurement.getDate().shiftedBy(-0.75 * meanDelay);
348             final SpacecraftState state = propagator.propagate(date);
349 
350             final double[][] jacobian = measurement.estimate(0, 0, new SpacecraftState[] { state }).getStateDerivatives(0);
351 
352             final double[][] finiteDifferencesJacobian =
353                     Differentiation.differentiate(new StateFunction() {
354                 public double[] value(final SpacecraftState state) {
355                     return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue();
356                 }
357             }, 1, propagator.getAttitudeProvider(),
358                OrbitType.CARTESIAN, PositionAngle.TRUE, 15.0, 3).value(state);
359 
360             Assert.assertEquals(finiteDifferencesJacobian.length, jacobian.length);
361             Assert.assertEquals(finiteDifferencesJacobian[0].length, jacobian[0].length);
362 
363             for (int i = 0; i < jacobian.length; ++i) {
364                 for (int j = 0; j < jacobian[i].length; ++j) {
365                     // check the values returned by getStateDerivatives() are correct
366                     maxRelativeError = FastMath.max(maxRelativeError,
367                                                     FastMath.abs((finiteDifferencesJacobian[i][j] - jacobian[i][j]) /
368                                                                  finiteDifferencesJacobian[i][j]));
369                 }
370             }
371 
372         }
373         Assert.assertEquals(0, maxRelativeError, 7.2e-8);
374 
375     }
376 
377     @Test
378     public void testParameterDerivativesWithModifier() {
379 
380         DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
381 
382         final DSSTPropagatorBuilder propagatorBuilder =
383                         context.createBuilder(true, 1.0e-6, 60.0, 0.001);
384 
385         // create perfect range rate measurements
386         final double groundClockDrift =  4.8e-9;
387         for (final GroundStation station : context.stations) {
388             station.getClockDriftDriver().setValue(groundClockDrift);
389         }
390         final double satClkDrift = 3.2e-10;
391         final DSSTRangeRateMeasurementCreatorementCreator.html#DSSTRangeRateMeasurementCreator">DSSTRangeRateMeasurementCreator creator = new DSSTRangeRateMeasurementCreator(context, false, satClkDrift);
392         creator.getSatellite().getClockDriftDriver().setSelected(true);
393         for (final GroundStation station : context.stations) {
394             station.getClockDriftDriver().setSelected(true);
395             station.getEastOffsetDriver().setSelected(true);
396             station.getNorthOffsetDriver().setSelected(true);
397             station.getZenithOffsetDriver().setSelected(true);
398         }
399         final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit,
400                                                                            propagatorBuilder);
401         final List<ObservedMeasurement<?>> measurements =
402                         DSSTEstimationTestUtils.createMeasurements(propagator,
403                                                                creator,
404                                                                1.0, 3.0, 300.0);
405         propagator.setSlaveMode();
406 
407         double maxRelativeError = 0;
408         for (final ObservedMeasurement<?> measurement : measurements) {
409 
410             final RangeRateTroposphericDelayModifier modifier = new RangeRateTroposphericDelayModifier(SaastamoinenModel.getStandardModel(), true);
411             ((RangeRate) measurement).addModifier(modifier);
412 
413             // parameter corresponding to station position offset
414             final GroundStation stationParameter = ((RangeRate) measurement).getStation();
415 
416             // We intentionally propagate to a date which is close to the
417             // real spacecraft state but is *not* the accurate date, by
418             // compensating only part of the downlink delay. This is done
419             // in order to validate the partial derivatives with respect
420             // to velocity. If we had chosen the proper state date, the
421             // range would have depended only on the current position but
422             // not on the current velocity.
423             final double          meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
424             final AbsoluteDate    date      = measurement.getDate().shiftedBy(-0.75 * meanDelay);
425             final SpacecraftState state     = propagator.propagate(date);
426             final ParameterDriver[] drivers = new ParameterDriver[] {
427                 stationParameter.getClockDriftDriver(),
428                 stationParameter.getEastOffsetDriver(),
429                 stationParameter.getNorthOffsetDriver(),
430                 stationParameter.getZenithOffsetDriver(),
431                 measurement.getSatellites().get(0).getClockDriftDriver()
432             };
433             for (int i = 0; i < drivers.length; ++i) {
434                 final double[] gradient  = measurement.estimate(0, 0, new SpacecraftState[] { state }).getParameterDerivatives(drivers[i]);
435                 Assert.assertEquals(1, measurement.getDimension());
436                 Assert.assertEquals(1, gradient.length);
437 
438                 final ParameterFunction dMkdP =
439                                 Differentiation.differentiate(new ParameterFunction() {
440                                     /** {@inheritDoc} */
441                                     @Override
442                                     public double value(final ParameterDriver parameterDriver) {
443                                         return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue()[0];
444                                     }
445                                 }, 3, 20.0 * drivers[i].getScale());
446                 final double ref = dMkdP.value(drivers[i]);
447                 maxRelativeError = FastMath.max(maxRelativeError, FastMath.abs((ref - gradient[0]) / ref));
448             }
449 
450         }
451         Assert.assertEquals(0, maxRelativeError, 8.3e-7);
452 
453     }
454 
455 }