1   /* Copyright 2002-2021 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.stat.descriptive.StreamingStatistics;
22  import org.hipparchus.util.FastMath;
23  import org.junit.Assert;
24  import org.junit.Test;
25  import org.orekit.estimation.Context;
26  import org.orekit.estimation.EstimationTestUtils;
27  import org.orekit.estimation.measurements.modifiers.RangeRateTroposphericDelayModifier;
28  import org.orekit.models.earth.troposphere.EstimatedTroposphericModel;
29  import org.orekit.models.earth.troposphere.GlobalMappingFunctionModel;
30  import org.orekit.models.earth.troposphere.SaastamoinenModel;
31  import org.orekit.orbits.OrbitType;
32  import org.orekit.orbits.PositionAngle;
33  import org.orekit.propagation.Propagator;
34  import org.orekit.propagation.SpacecraftState;
35  import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
36  import org.orekit.time.AbsoluteDate;
37  import org.orekit.utils.Constants;
38  import org.orekit.utils.Differentiation;
39  import org.orekit.utils.ParameterDriver;
40  import org.orekit.utils.ParameterFunction;
41  import org.orekit.utils.StateFunction;
42  
43  public class RangeRateTest {
44  
45      /** Compare observed values and estimated values.
46       *  Both are calculated with a different algorithm.
47       *  One-way measurements.
48       */
49      @Test
50      public void testValuesOneWay() {
51  
52          Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
53  
54          final NumericalPropagatorBuilder propagatorBuilder =
55                          context.createBuilder(OrbitType.EQUINOCTIAL, PositionAngle.TRUE, false,
56                                                1.0e-6, 60.0, 0.001);
57  
58          // Create perfect right-ascension/declination measurements
59          final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
60                                                                             propagatorBuilder);
61          final double satClkDrift = 3.2e-10;
62          final List<ObservedMeasurement<?>> measurements =
63                          EstimationTestUtils.createMeasurements(propagator,
64                                                                 new RangeRateMeasurementCreator(context, false, satClkDrift),
65                                                                 1.0, 3.0, 300.0);
66  
67          propagator.clearStepHandlers();
68  
69          // Prepare statistics for values difference
70          final StreamingStatistics diffStat = new StreamingStatistics();
71  
72          for (final ObservedMeasurement<?> measurement : measurements) {
73  
74              // Propagate to measurement date
75              final AbsoluteDate datemeas  = measurement.getDate();
76              SpacecraftState    state     = propagator.propagate(datemeas);
77              
78              // Estimate the AZEL value
79              final EstimatedMeasurement<?> estimated = measurement.estimate(0, 0, new SpacecraftState[] { state });
80              
81              // Store the difference between estimated and observed values in the stats
82              diffStat.addValue(FastMath.abs(estimated.getEstimatedValue()[0] - measurement.getObservedValue()[0]));
83          }
84  
85          // Mean and std errors check
86          Assert.assertEquals(0.0, diffStat.getMean(), 6.5e-8);
87          Assert.assertEquals(0.0, diffStat.getStandardDeviation(), 5.5e-8);
88      }
89      
90      /** Compare observed values and estimated values.
91       *  Both are calculated with a different algorithm.
92       *  Two-ways measurements.
93       */
94      @Test
95      public void testValuesTwoWays() {
96  
97          Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
98  
99          final NumericalPropagatorBuilder propagatorBuilder =
100                         context.createBuilder(OrbitType.EQUINOCTIAL, PositionAngle.TRUE, false,
101                                               1.0e-6, 60.0, 0.001);
102 
103         // Create perfect right-ascension/declination measurements
104         final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
105                                                                            propagatorBuilder);
106         final double satClkDrift = 3.2e-10;
107         final List<ObservedMeasurement<?>> measurements =
108                         EstimationTestUtils.createMeasurements(propagator,
109                                                                new RangeRateMeasurementCreator(context, true, satClkDrift),
110                                                                1.0, 3.0, 300.0);
111 
112         propagator.clearStepHandlers();
113 
114         // Prepare statistics for values difference
115         final StreamingStatistics diffStat = new StreamingStatistics();
116 
117         for (final ObservedMeasurement<?> measurement : measurements) {
118 
119             // Propagate to measurement date
120             final AbsoluteDate datemeas  = measurement.getDate();
121             SpacecraftState    state     = propagator.propagate(datemeas);
122             
123             // Estimate the AZEL value
124             final EstimatedMeasurement<?> estimated = measurement.estimate(0, 0, new SpacecraftState[] { state });
125             
126             // Store the difference between estimated and observed values in the stats
127             diffStat.addValue(FastMath.abs(estimated.getEstimatedValue()[0] - measurement.getObservedValue()[0]));
128         }
129 
130         // Mean and std errors check
131         Assert.assertEquals(0.0, diffStat.getMean(), 6.5e-8);
132         Assert.assertEquals(0.0, diffStat.getStandardDeviation(), 5.5e-8);
133     }
134     
135     /** Test the values of the state derivatives using a numerical
136      * finite differences calculation as a reference.
137      * One way measurements.
138      */
139     @Test
140     public void testStateDerivativesOneWay() {
141 
142         Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
143 
144         final NumericalPropagatorBuilder propagatorBuilder =
145                         context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
146                                               1.0e-6, 60.0, 0.001);
147 
148         // create perfect range rate measurements
149         final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
150                                                                            propagatorBuilder);
151         final double satClkDrift = 3.2e-10;
152         final List<ObservedMeasurement<?>> measurements =
153                         EstimationTestUtils.createMeasurements(propagator,
154                                                                new RangeRateMeasurementCreator(context, false, satClkDrift),
155                                                                1.0, 3.0, 300.0);
156         for (final ObservedMeasurement<?> m : measurements) {
157             Assert.assertFalse(((RangeRate) m).isTwoWay());
158         }
159         propagator.clearStepHandlers();
160 
161         double maxRelativeError = 0;
162         for (final ObservedMeasurement<?> measurement : measurements) {
163 
164             final double          meanDelay = 1; // measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
165             final AbsoluteDate    date      = measurement.getDate().shiftedBy(-0.75 * meanDelay);
166             final SpacecraftState state = propagator.propagate(date);
167 
168             final EstimatedMeasurement<?> estimated = measurement.estimate(0, 0, new SpacecraftState[] { state });
169             Assert.assertEquals(2, estimated.getParticipants().length);
170             final double[][] jacobian = estimated.getStateDerivatives(0);
171 
172             final double[][] finiteDifferencesJacobian =
173                     Differentiation.differentiate(new StateFunction() {
174                 public double[] value(final SpacecraftState state) {
175                     return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue();
176                 }
177             }, 1, propagator.getAttitudeProvider(),
178                OrbitType.CARTESIAN, PositionAngle.TRUE, 15.0, 3).value(state);
179 
180             Assert.assertEquals(finiteDifferencesJacobian.length, jacobian.length);
181             Assert.assertEquals(finiteDifferencesJacobian[0].length, jacobian[0].length);
182 
183             for (int i = 0; i < jacobian.length; ++i) {
184                 for (int j = 0; j < jacobian[i].length; ++j) {
185                     // check the values returned by getStateDerivatives() are correct
186                     maxRelativeError = FastMath.max(maxRelativeError,
187                                                     FastMath.abs((finiteDifferencesJacobian[i][j] - jacobian[i][j]) /
188                                                                  finiteDifferencesJacobian[i][j]));
189                 }
190             }
191 
192         }
193         Assert.assertEquals(0, maxRelativeError, 1.5e-8);
194 
195     }
196 
197     /** Test the values of the state derivatives using a numerical
198      * finite differences calculation as a reference.
199      * Two-ways measurements.
200      */
201     @Test
202     public void testStateDerivativesTwoWays() {
203 
204         Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
205 
206         final NumericalPropagatorBuilder propagatorBuilder =
207                         context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
208                                               1.0e-6, 60.0, 0.001);
209 
210         // create perfect range rate measurements
211         final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
212                                                                            propagatorBuilder);
213         final double satClkDrift = 3.2e-10;
214         final List<ObservedMeasurement<?>> measurements =
215                         EstimationTestUtils.createMeasurements(propagator,
216                                                                new RangeRateMeasurementCreator(context, true, satClkDrift),
217                                                                1.0, 3.0, 300.0);
218         for (final ObservedMeasurement<?> m : measurements) {
219             Assert.assertTrue(((RangeRate) m).isTwoWay());
220         }
221         propagator.clearStepHandlers();
222 
223         double maxRelativeError = 0;
224         for (final ObservedMeasurement<?> measurement : measurements) {
225 
226             //
227             //final AbsoluteDate date = measurement.getDate();
228             final double          meanDelay = 1; // measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
229             final AbsoluteDate    date      = measurement.getDate().shiftedBy(-0.75 * meanDelay);
230             final SpacecraftState state     = propagator.propagate(date);
231 
232             final EstimatedMeasurement<?> estimated = measurement.estimate(0, 0, new SpacecraftState[] { state });
233             Assert.assertEquals(3, estimated.getParticipants().length);
234             final double[][] jacobian = estimated.getStateDerivatives(0);
235 
236             final double[][] finiteDifferencesJacobian =
237                     Differentiation.differentiate(new StateFunction() {
238                 public double[] value(final SpacecraftState state) {
239                     return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue();
240                 }
241             }, 1, propagator.getAttitudeProvider(),
242                OrbitType.CARTESIAN, PositionAngle.TRUE, 15.0, 3).value(state);
243 
244             Assert.assertEquals(finiteDifferencesJacobian.length, jacobian.length);
245             Assert.assertEquals(finiteDifferencesJacobian[0].length, jacobian[0].length);
246 
247             for (int i = 0; i < jacobian.length; ++i) {
248                 for (int j = 0; j < jacobian[i].length; ++j) {
249                     // check the values returned by getStateDerivatives() are correct
250                     maxRelativeError = FastMath.max(maxRelativeError,
251                                                     FastMath.abs((finiteDifferencesJacobian[i][j] - jacobian[i][j]) /
252                                                                  finiteDifferencesJacobian[i][j]));
253                 }
254             }
255 
256         }
257         Assert.assertEquals(0, maxRelativeError, 2.1e-7);
258 
259     }
260 
261     /** Test the values of the parameters' derivatives using a numerical
262      * finite differences calculation as a reference.
263      * One-way measurements.
264      */
265     @Test
266     public void testParameterDerivativesOneWay() {
267 
268         Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
269 
270         final NumericalPropagatorBuilder propagatorBuilder =
271                         context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
272                                               1.0e-6, 60.0, 0.001);
273 
274         // create perfect range rate measurements
275         final double groundClockDrift =  4.8e-9;
276         for (final GroundStation station : context.stations) {
277             station.getClockDriftDriver().setValue(groundClockDrift);
278         }
279         final double satClkDrift = 3.2e-10;
280         final RangeRateMeasurementCreator creator = new RangeRateMeasurementCreator(context, false, satClkDrift);
281         creator.getSatellite().getClockDriftDriver().setSelected(true);
282         for (final GroundStation station : context.stations) {
283             station.getClockOffsetDriver().setSelected(true);
284             station.getClockDriftDriver().setSelected(true);
285             station.getEastOffsetDriver().setSelected(true);
286             station.getNorthOffsetDriver().setSelected(true);
287             station.getZenithOffsetDriver().setSelected(true);
288         }
289         final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
290                                                                            propagatorBuilder);
291 
292 
293         
294         final List<ObservedMeasurement<?>> measurements =
295                         EstimationTestUtils.createMeasurements(propagator,
296                                                                creator,
297                                                                1.0, 3.0, 300.0);
298         propagator.clearStepHandlers();
299 
300         double maxRelativeError = 0;
301         for (final ObservedMeasurement<?> measurement : measurements) {
302 
303             // parameter corresponding to station position offset
304             final GroundStation stationParameter = ((RangeRate) measurement).getStation();
305 
306             // We intentionally propagate to a date which is close to the
307             // real spacecraft state but is *not* the accurate date, by
308             // compensating only part of the downlink delay. This is done
309             // in order to validate the partial derivatives with respect
310             // to velocity. If we had chosen the proper state date, the
311             // range would have depended only on the current position but
312             // not on the current velocity.
313             final double          meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
314             final AbsoluteDate    date      = measurement.getDate().shiftedBy(-0.75 * meanDelay);
315             final SpacecraftState state     = propagator.propagate(date);
316             final ParameterDriver[] drivers = new ParameterDriver[] {
317                 stationParameter.getClockDriftDriver(),
318                 stationParameter.getEastOffsetDriver(),
319                 stationParameter.getNorthOffsetDriver(),
320                 stationParameter.getZenithOffsetDriver(),
321                 measurement.getSatellites().get(0).getClockDriftDriver()
322             };
323             for (int i = 0; i < drivers.length; ++i) {
324                 final double[] gradient  = measurement.estimate(0, 0, new SpacecraftState[] { state }).getParameterDerivatives(drivers[i]);
325                 Assert.assertEquals(1, measurement.getDimension());
326                 Assert.assertEquals(1, gradient.length);
327 
328                 final ParameterFunction dMkdP =
329                                 Differentiation.differentiate(new ParameterFunction() {
330                                     /** {@inheritDoc} */
331                                     @Override
332                                     public double value(final ParameterDriver parameterDriver) {
333                                         return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue()[0];
334                                     }
335                                 }, 3, 20.0 * drivers[i].getScale());
336                 final double ref = dMkdP.value(drivers[i]);
337                 maxRelativeError = FastMath.max(maxRelativeError, FastMath.abs((ref - gradient[0]) / ref));
338             }
339 
340         }
341         Assert.assertEquals(0, maxRelativeError, 1.2e-6);
342 
343     }
344 
345     /** Test the values of the parameters' derivatives using a numerical
346      * finite differences calculation as a reference.
347      * Two-ways measurements.
348      */
349     @Test
350     public void testParameterDerivativesTwoWays() {
351 
352         Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
353 
354         final NumericalPropagatorBuilder propagatorBuilder =
355                         context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
356                                               1.0e-6, 60.0, 0.001);
357 
358         // create perfect range rate measurements
359         final double groundClockDrift =  4.8e-9;
360         for (final GroundStation station : context.stations) {
361             station.getClockDriftDriver().setValue(groundClockDrift);
362         }
363         final double satClkDrift = 3.2e-10;
364         final RangeRateMeasurementCreator creator = new RangeRateMeasurementCreator(context, false, satClkDrift);
365         for (final GroundStation station : context.stations) {
366             station.getClockOffsetDriver().setSelected(true);
367             station.getEastOffsetDriver().setSelected(true);
368             station.getNorthOffsetDriver().setSelected(true);
369             station.getZenithOffsetDriver().setSelected(true);
370         }
371 
372         final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
373                                                                            propagatorBuilder);
374         
375 
376         final List<ObservedMeasurement<?>> measurements =
377                         EstimationTestUtils.createMeasurements(propagator,
378                                                                creator,
379                                                                1.0, 3.0, 300.0);
380         propagator.clearStepHandlers();
381 
382         double maxRelativeError = 0;
383         for (final ObservedMeasurement<?> measurement : measurements) {
384 
385             // parameter corresponding to station position offset
386             final GroundStation stationParameter = ((RangeRate) measurement).getStation();
387 
388             // We intentionally propagate to a date which is close to the
389             // real spacecraft state but is *not* the accurate date, by
390             // compensating only part of the downlink delay. This is done
391             // in order to validate the partial derivatives with respect
392             // to velocity. If we had chosen the proper state date, the
393             // range would have depended only on the current position but
394             // not on the current velocity.
395             final double          meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
396             final AbsoluteDate    date      = measurement.getDate().shiftedBy(-0.75 * meanDelay);
397             final SpacecraftState state     = propagator.propagate(date);
398             final ParameterDriver[] drivers = new ParameterDriver[] {
399                 stationParameter.getEastOffsetDriver(),
400                 stationParameter.getNorthOffsetDriver(),
401                 stationParameter.getZenithOffsetDriver(),
402             };
403             for (int i = 0; i < drivers.length; ++i) {
404                 final double[] gradient  = measurement.estimate(0, 0, new SpacecraftState[] { state }).getParameterDerivatives(drivers[i]);
405                 Assert.assertEquals(1, measurement.getDimension());
406                 Assert.assertEquals(1, gradient.length);
407 
408                 final ParameterFunction dMkdP =
409                                 Differentiation.differentiate(new ParameterFunction() {
410                                     /** {@inheritDoc} */
411                                     @Override
412                                     public double value(final ParameterDriver parameterDriver) {
413                                         return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue()[0];
414                                     }
415                                 }, 3, 20.0 * drivers[i].getScale());
416                 final double ref = dMkdP.value(drivers[i]);
417                 maxRelativeError = FastMath.max(maxRelativeError, FastMath.abs((ref - gradient[0]) / ref));
418             }
419 
420         }
421         Assert.assertEquals(0, maxRelativeError, 1.2e-6);
422 
423     }
424 
425     /** Test the values of the state derivatives using a numerical
426      * finite differences calculation as a reference.
427      * One-way measurements with modifiers (tropospheric corrections).
428      */
429     @Test
430     public void testStateDerivativesWithModifier() {
431 
432         Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
433 
434         final NumericalPropagatorBuilder propagatorBuilder =
435                         context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
436                                               1.0e-6, 60.0, 0.001);
437 
438         // create perfect range rate measurements
439         final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
440                                                                            propagatorBuilder);
441         final double groundClockDrift =  4.8e-9;
442         for (final GroundStation station : context.stations) {
443             station.getClockDriftDriver().setValue(groundClockDrift);
444         }
445         final double satClkDrift = 3.2e-10;
446         final List<ObservedMeasurement<?>> measurements =
447                         EstimationTestUtils.createMeasurements(propagator,
448                                                                new RangeRateMeasurementCreator(context, false, satClkDrift),
449                                                                1.0, 3.0, 300.0);
450         propagator.clearStepHandlers();
451 
452         double maxRelativeError = 0;
453         for (final ObservedMeasurement<?> measurement : measurements) {
454 
455             final RangeRateTroposphericDelayModifier modifier = new RangeRateTroposphericDelayModifier(SaastamoinenModel.getStandardModel(), true);
456             ((RangeRate) measurement).addModifier(modifier);
457 
458             //
459             //final AbsoluteDate date = measurement.getDate();
460             final double          meanDelay = 1; // measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
461             final AbsoluteDate    date      = measurement.getDate().shiftedBy(-0.75 * meanDelay);
462             final SpacecraftState state = propagator.propagate(date);
463 
464             final double[][] jacobian = measurement.estimate(0, 0, new SpacecraftState[] { state }).getStateDerivatives(0);
465 
466             final double[][] finiteDifferencesJacobian =
467                     Differentiation.differentiate(new StateFunction() {
468                 public double[] value(final SpacecraftState state) {
469                     return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue();
470                 }
471             }, 1, propagator.getAttitudeProvider(),
472                OrbitType.CARTESIAN, PositionAngle.TRUE, 15.0, 3).value(state);
473 
474             Assert.assertEquals(finiteDifferencesJacobian.length, jacobian.length);
475             Assert.assertEquals(finiteDifferencesJacobian[0].length, jacobian[0].length);
476 
477             for (int i = 0; i < jacobian.length; ++i) {
478                 for (int j = 0; j < jacobian[i].length; ++j) {
479                     // check the values returned by getStateDerivatives() are correct
480                     maxRelativeError = FastMath.max(maxRelativeError,
481                                                     FastMath.abs((finiteDifferencesJacobian[i][j] - jacobian[i][j]) /
482                                                                  finiteDifferencesJacobian[i][j]));
483                 }
484             }
485 
486         }
487         Assert.assertEquals(0, maxRelativeError, 1.4e-7);
488 
489     }
490 
491     /** Test the values of the state derivatives using a numerical
492      * finite differences calculation as a reference.
493      * One-way measurements with estimated modifiers (tropospheric corrections).
494      */
495     @Test
496     public void testStateDerivativesWithEstimatedModifier() {
497 
498         Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
499 
500         final NumericalPropagatorBuilder propagatorBuilder =
501                         context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
502                                               1.0e-6, 60.0, 0.001);
503 
504         // create perfect range rate measurements
505         final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
506                                                                            propagatorBuilder);
507 
508         final double groundClockDrift =  4.8e-9;
509         for (final GroundStation station : context.stations) {
510             station.getClockDriftDriver().setValue(groundClockDrift);
511         }
512         final double satClkDrift = 3.2e-10;
513         final List<ObservedMeasurement<?>> measurements =
514                         EstimationTestUtils.createMeasurements(propagator,
515                                                                new RangeRateMeasurementCreator(context, false, satClkDrift),
516                                                                1.0, 3.0, 300.0);
517         propagator.clearStepHandlers();
518 
519         double maxRelativeError = 0;
520         for (final ObservedMeasurement<?> measurement : measurements) {
521 
522             // Add modifiers if test implies it
523             final GlobalMappingFunctionModel mappingFunction = new GlobalMappingFunctionModel();
524             final EstimatedTroposphericModel tropoModel     = new EstimatedTroposphericModel(mappingFunction, 5.0);
525 
526             final RangeRateTroposphericDelayModifier modifier = new RangeRateTroposphericDelayModifier(tropoModel, true);
527             ((RangeRate) measurement).addModifier(modifier);
528 
529             //
530             //final AbsoluteDate date = measurement.getDate();
531             final double          meanDelay = 1; // measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
532             final AbsoluteDate    date      = measurement.getDate().shiftedBy(-0.75 * meanDelay);
533             final SpacecraftState state = propagator.propagate(date);
534 
535             final double[][] jacobian = measurement.estimate(0, 0, new SpacecraftState[] { state }).getStateDerivatives(0);
536 
537             final double[][] finiteDifferencesJacobian =
538                     Differentiation.differentiate(new StateFunction() {
539                 public double[] value(final SpacecraftState state) {
540                     return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue();
541                 }
542             }, 1, propagator.getAttitudeProvider(),
543                OrbitType.CARTESIAN, PositionAngle.TRUE, 15.0, 3).value(state);
544 
545             Assert.assertEquals(finiteDifferencesJacobian.length, jacobian.length);
546             Assert.assertEquals(finiteDifferencesJacobian[0].length, jacobian[0].length);
547 
548             for (int i = 0; i < jacobian.length; ++i) {
549                 for (int j = 0; j < jacobian[i].length; ++j) {
550                     // check the values returned by getStateDerivatives() are correct
551                     maxRelativeError = FastMath.max(maxRelativeError,
552                                                     FastMath.abs((finiteDifferencesJacobian[i][j] - jacobian[i][j]) /
553                                                                  finiteDifferencesJacobian[i][j]));
554                 }
555             }
556 
557         }
558         Assert.assertEquals(0, maxRelativeError, 3.1e-7);
559 
560     }
561 
562     /** Test the values of the parameters' derivatives using a numerical
563      * finite differences calculation as a reference.
564      * One-way measurements with modifiers (tropospheric corrections).
565      */
566     @Test
567     public void testParameterDerivativesWithModifier() {
568 
569         Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
570 
571         final NumericalPropagatorBuilder propagatorBuilder =
572                         context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
573                                               1.0e-6, 60.0, 0.001);
574 
575         // create perfect range rate measurements
576         final double groundClockDrift =  4.8e-9;
577         for (final GroundStation station : context.stations) {
578             station.getClockDriftDriver().setValue(groundClockDrift);
579         }
580         final double satClkDrift = 3.2e-10;
581         final RangeRateMeasurementCreator creator = new RangeRateMeasurementCreator(context, false, satClkDrift);
582         creator.getSatellite().getClockDriftDriver().setSelected(true);
583         for (final GroundStation station : context.stations) {
584             station.getClockOffsetDriver().setSelected(true);
585             station.getClockDriftDriver().setSelected(true);
586             station.getEastOffsetDriver().setSelected(true);
587             station.getNorthOffsetDriver().setSelected(true);
588             station.getZenithOffsetDriver().setSelected(true);
589         }
590         final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
591                                                                            propagatorBuilder);
592 
593         final List<ObservedMeasurement<?>> measurements =
594                         EstimationTestUtils.createMeasurements(propagator,
595                                                                creator,
596                                                                1.0, 3.0, 300.0);
597         propagator.clearStepHandlers();
598 
599         double maxRelativeError = 0;
600         for (final ObservedMeasurement<?> measurement : measurements) {
601 
602             final RangeRateTroposphericDelayModifier modifier = new RangeRateTroposphericDelayModifier(SaastamoinenModel.getStandardModel(), true);
603             ((RangeRate) measurement).addModifier(modifier);
604 
605             // parameter corresponding to station position offset
606             final GroundStation stationParameter = ((RangeRate) measurement).getStation();
607 
608             // We intentionally propagate to a date which is close to the
609             // real spacecraft state but is *not* the accurate date, by
610             // compensating only part of the downlink delay. This is done
611             // in order to validate the partial derivatives with respect
612             // to velocity. If we had chosen the proper state date, the
613             // range would have depended only on the current position but
614             // not on the current velocity.
615             final double          meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
616             final AbsoluteDate    date      = measurement.getDate().shiftedBy(-0.75 * meanDelay);
617             final SpacecraftState state     = propagator.propagate(date);
618             final ParameterDriver[] drivers = new ParameterDriver[] {
619                 stationParameter.getClockDriftDriver(),
620                 stationParameter.getEastOffsetDriver(),
621                 stationParameter.getNorthOffsetDriver(),
622                 stationParameter.getZenithOffsetDriver(),
623                 measurement.getSatellites().get(0).getClockDriftDriver()
624             };
625             for (int i = 0; i < drivers.length; ++i) {
626                 final double[] gradient  = measurement.estimate(0, 0, new SpacecraftState[] { state }).getParameterDerivatives(drivers[i]);
627                 Assert.assertEquals(1, measurement.getDimension());
628                 Assert.assertEquals(1, gradient.length);
629 
630                 final ParameterFunction dMkdP =
631                                 Differentiation.differentiate(new ParameterFunction() {
632                                     /** {@inheritDoc} */
633                                     @Override
634                                     public double value(final ParameterDriver parameterDriver) {
635                                         return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue()[0];
636                                     }
637                                 }, 3, 20.0 * drivers[i].getScale());
638                 final double ref = dMkdP.value(drivers[i]);
639                 maxRelativeError = FastMath.max(maxRelativeError, FastMath.abs((ref - gradient[0]) / ref));
640             }
641 
642         }
643         Assert.assertEquals(0, maxRelativeError, 1.2e-6);
644 
645     }
646 
647     /** Test the values of the parameters' derivatives using a numerical
648      * finite differences calculation as a reference.
649      * One-way measurements with estimated modifiers (tropospheric corrections).
650      */
651     @Test
652     public void testParameterDerivativesWithEstimatedModifier() {
653 
654         Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
655 
656         final NumericalPropagatorBuilder propagatorBuilder =
657                         context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
658                                               1.0e-6, 60.0, 0.001);
659 
660         final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
661                                                                            propagatorBuilder);
662 
663         final double groundClockDrift =  4.8e-9;
664         for (final GroundStation station : context.stations) {
665             station.getClockDriftDriver().setValue(groundClockDrift);
666         }
667         final double satClkDrift = 3.2e-10;
668         final List<ObservedMeasurement<?>> measurements =
669                         EstimationTestUtils.createMeasurements(propagator,
670                                                                new RangeRateMeasurementCreator(context, false, satClkDrift),
671                                                                1.0, 3.0, 300.0);
672         propagator.clearStepHandlers();
673 
674         double maxRelativeError = 0;
675         for (final ObservedMeasurement<?> measurement : measurements) {
676 
677             // Add modifiers if test implies it
678             final GlobalMappingFunctionModel mappingFunction = new GlobalMappingFunctionModel();
679             final EstimatedTroposphericModel tropoModel     = new EstimatedTroposphericModel(mappingFunction, 10.0);
680             
681             final List<ParameterDriver> parameters = tropoModel.getParametersDrivers();
682             for (ParameterDriver driver : parameters) {
683                 driver.setSelected(true);
684             }
685 
686             final RangeRateTroposphericDelayModifier modifier = new RangeRateTroposphericDelayModifier(tropoModel, true);
687             ((RangeRate) measurement).addModifier(modifier);
688 
689             // We intentionally propagate to a date which is close to the
690             // real spacecraft state but is *not* the accurate date, by
691             // compensating only part of the downlink delay. This is done
692             // in order to validate the partial derivatives with respect
693             // to velocity. If we had chosen the proper state date, the
694             // range would have depended only on the current position but
695             // not on the current velocity.
696             final double          meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
697             final AbsoluteDate    date      = measurement.getDate().shiftedBy(-0.75 * meanDelay);
698             final SpacecraftState state     = propagator.propagate(date);
699 
700             final ParameterDriver[] drivers = new ParameterDriver[] {
701                 parameters.get(0)
702             };
703             for (int i = 0; i < 1; ++i) {
704                 final double[] gradient  = measurement.estimate(0, 0, new SpacecraftState[] { state }).getParameterDerivatives(drivers[i]);
705                 Assert.assertEquals(1, measurement.getDimension());
706                 Assert.assertEquals(1, gradient.length);
707 
708                 final ParameterFunction dMkdP =
709                                 Differentiation.differentiate(new ParameterFunction() {
710                                     /** {@inheritDoc} */
711                                     @Override
712                                     public double value(final ParameterDriver parameterDriver) {
713                                         return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue()[0];
714                                     }
715                                 }, 3, 0.1 * drivers[i].getScale());
716                 final double ref = dMkdP.value(drivers[i]);
717                 maxRelativeError = FastMath.max(maxRelativeError, FastMath.abs((ref - gradient[0]) / ref));
718             }
719 
720         }
721         Assert.assertEquals(0, maxRelativeError, 2.2e-7);
722 
723     }
724 
725 }
726 
727