1   /* Copyright 2002-2019 CS Systèmes d'Information
2    * Licensed to CS Systèmes d'Information (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.modifiers;
18  
19  import java.util.Arrays;
20  import java.util.List;
21  
22  import org.hipparchus.Field;
23  import org.hipparchus.RealFieldElement;
24  import org.hipparchus.analysis.differentiation.DerivativeStructure;
25  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
26  import org.hipparchus.geometry.euclidean.threed.Vector3D;
27  import org.orekit.estimation.measurements.EstimatedMeasurement;
28  import org.orekit.estimation.measurements.EstimationModifier;
29  import org.orekit.estimation.measurements.GroundStation;
30  import org.orekit.estimation.measurements.RangeRate;
31  import org.orekit.models.earth.DiscreteTroposphericModel;
32  import org.orekit.models.earth.TroposphericModel;
33  import org.orekit.orbits.OrbitType;
34  import org.orekit.orbits.PositionAngle;
35  import org.orekit.propagation.FieldSpacecraftState;
36  import org.orekit.propagation.Propagator;
37  import org.orekit.propagation.SpacecraftState;
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  /** Class modifying theoretical range-rate measurements with tropospheric delay.
44   * The effect of tropospheric correction on the range-rate is directly computed
45   * through the computation of the tropospheric delay difference with respect to
46   * time.
47   *
48   * In general, for GNSS, VLBI, ... there is hardly any frequency dependence in the delay.
49   * For SLR techniques however, the frequency dependence is sensitive.
50   *
51   * @author Joris Olympio
52   * @since 8.0
53   */
54  public class RangeRateTroposphericDelayModifier implements EstimationModifier<RangeRate> {
55  
56      /** Tropospheric delay model. */
57      private final DiscreteTroposphericModel tropoModel;
58  
59      /** Two-way measurement factor. */
60      private final double fTwoWay;
61  
62      /** Constructor.
63       *
64       * @param model  Tropospheric delay model appropriate for the current range-rate measurement method.
65       * @param tw     Flag indicating whether the measurement is two-way.
66       */
67      public RangeRateTroposphericDelayModifier(final DiscreteTroposphericModel model, final boolean tw) {
68          tropoModel = model;
69          if (tw) {
70              fTwoWay = 2.;
71          } else {
72              fTwoWay = 1.;
73          }
74      }
75  
76      /** Get the station height above mean sea level.
77       *
78       * @param station  ground station (or measuring station)
79       * @return the measuring station height above sea level, m
80       */
81      private double getStationHeightAMSL(final GroundStation station) {
82          // FIXME heigth should be computed with respect to geoid WGS84+GUND = EGM2008 for example
83          final double height = station.getBaseFrame().getPoint().getAltitude();
84          return height;
85      }
86  
87      /** Get the station height above mean sea level.
88      * @param <T> type of the element
89      * @param field field of the elements
90      * @param station  ground station (or measuring station)
91      * @return the measuring station height above sea level, m
92      */
93      private <T extends RealFieldElement<T>> T getStationHeightAMSL(final Field<T> field, final GroundStation station) {
94          // FIXME heigth should be computed with respect to geoid WGS84+GUND = EGM2008 for example
95          final T height = station.getBaseFrame().getPoint(field).getAltitude();
96          return height;
97      }
98  
99      /** Compute the measurement error due to Troposphere.
100      * @param station station
101      * @param state spacecraft state
102      * @return the measurement error due to Troposphere
103      */
104     public double rangeRateErrorTroposphericModel(final GroundStation station,
105                                                   final SpacecraftState state) {
106         // The effect of tropospheric correction on the range rate is
107         // computed using finite differences.
108 
109         final double dt = 10; // s
110 
111         // station altitude AMSL in meters
112         final double height = getStationHeightAMSL(station);
113 
114         // spacecraft position and elevation as seen from the ground station
115         final Vector3D position = state.getPVCoordinates().getPosition();
116 
117         // elevation
118         final double elevation1 = station.getBaseFrame().getElevation(position,
119                                                                       state.getFrame(),
120                                                                       state.getDate());
121 
122         // only consider measures above the horizon
123         if (elevation1 > 0) {
124             // tropospheric delay in meters
125             final double d1 = tropoModel.pathDelay(elevation1, height, tropoModel.getParameters(), state.getDate());
126 
127             // propagate spacecraft state forward by dt
128             final SpacecraftState state2 = state.shiftedBy(dt);
129 
130             // spacecraft position and elevation as seen from the ground station
131             final Vector3D position2 = state2.getPVCoordinates().getPosition();
132 
133             // elevation
134             final double elevation2 = station.getBaseFrame().getElevation(position2,
135                                                                           state2.getFrame(),
136                                                                           state2.getDate());
137 
138             // tropospheric delay dt after
139             final double d2 = tropoModel.pathDelay(elevation2, height, tropoModel.getParameters(), state2.getDate());
140 
141             return fTwoWay * (d2 - d1) / dt;
142         }
143 
144         return 0;
145     }
146 
147 
148     /** Compute the measurement error due to Troposphere.
149      * @param <T> type of the element
150      * @param station station
151      * @param state spacecraft state
152      * @param parameters tropospheric model parameters
153      * @return the measurement error due to Troposphere
154      */
155     public <T extends RealFieldElement<T>> T rangeRateErrorTroposphericModel(final GroundStation station,
156                                                                              final FieldSpacecraftState<T> state,
157                                                                              final T[] parameters) {
158         // Field
159         final Field<T> field = state.getDate().getField();
160         final T zero         = field.getZero();
161 
162         // The effect of tropospheric correction on the range rate is
163         // computed using finite differences.
164 
165         final double dt = 10; // s
166 
167         // station altitude AMSL in meters
168         final T height = getStationHeightAMSL(field, station);
169 
170         // spacecraft position and elevation as seen from the ground station
171         final FieldVector3D<T> position     = state.getPVCoordinates().getPosition();
172         final T elevation1                  = station.getBaseFrame().getElevation(position,
173                                                                                   state.getFrame(),
174                                                                                   state.getDate());
175 
176         // only consider measures above the horizon
177         if (elevation1.getReal() > 0) {
178             // tropospheric delay in meters
179             final T d1 = tropoModel.pathDelay(elevation1, height, parameters, state.getDate());
180 
181             // propagate spacecraft state forward by dt
182             final FieldSpacecraftState<T> state2 = state.shiftedBy(dt);
183 
184             // spacecraft position and elevation as seen from the ground station
185             final FieldVector3D<T> position2     = state2.getPVCoordinates().getPosition();
186 
187             // elevation
188             final T elevation2 = station.getBaseFrame().getElevation(position2,
189                                                                      state2.getFrame(),
190                                                                      state2.getDate());
191 
192 
193             // tropospheric delay dt after
194             final T d2 = tropoModel.pathDelay(elevation2, height, parameters, state2.getDate());
195 
196             return (d2.subtract(d1)).divide(dt).multiply(fTwoWay);
197         }
198 
199         return zero;
200     }
201 
202     /** Compute the Jacobian of the delay term wrt state.
203     *
204     * @param station station
205     * @param refstate spacecraft state
206     * @return Jacobian of the delay wrt state
207     */
208     private double[][] rangeRateErrorJacobianState(final GroundStation station,
209                                                    final SpacecraftState refstate) {
210         final double[][] finiteDifferencesJacobian =
211                         Differentiation.differentiate(new StateFunction() {
212                             public double[] value(final SpacecraftState state) {
213                                 // evaluate target's elevation with a changed target position
214                                 final double value = rangeRateErrorTroposphericModel(station, state);
215 
216                                 return new double[] {value };
217                             }
218                         }, 1, Propagator.DEFAULT_LAW, OrbitType.CARTESIAN,
219                         PositionAngle.TRUE, 15.0, 3).value(refstate);
220 
221         return finiteDifferencesJacobian;
222     }
223 
224     /** Compute the Jacobian of the delay term wrt state using
225     * automatic differentiation.
226     *
227     * @param derivatives tropospheric delay derivatives
228     * @param freeStateParameters dimension of the state.
229     *
230     * @return Jacobian of the delay wrt state
231     */
232     private double[][] rangeRateErrorJacobianState(final double[] derivatives, final int freeStateParameters) {
233         final double[][] finiteDifferencesJacobian = new double[1][6];
234         for (int i = 0; i < freeStateParameters; i++) {
235             // First element is the value of the delay
236             finiteDifferencesJacobian[0][i] = derivatives[i + 1];
237         }
238         return finiteDifferencesJacobian;
239     }
240 
241     /** Compute the derivative of the delay term wrt parameters.
242     *
243     * @param station ground station
244     * @param driver driver for the station offset parameter
245     * @param state spacecraft state
246     * @return derivative of the delay wrt station offset parameter
247     */
248     private double rangeRateErrorParameterDerivative(final GroundStation station,
249                                                      final ParameterDriver driver,
250                                                      final SpacecraftState state) {
251 
252         final ParameterFunctionParameterFunction">ParameterFunction rangeError = new ParameterFunction() {
253             /** {@inheritDoc} */
254             @Override
255             public double value(final ParameterDriver parameterDriver) {
256                 return rangeRateErrorTroposphericModel(station, state);
257             }
258         };
259 
260         final ParameterFunction rangeErrorDerivative =
261                         Differentiation.differentiate(rangeError, 3, 10.0 * driver.getScale());
262 
263         return rangeErrorDerivative.value(driver);
264 
265     }
266 
267     /** Compute the derivative of the delay term wrt parameters using
268     * automatic differentiation.
269     *
270     * @param derivatives tropospheric delay derivatives
271     * @param freeStateParameters dimension of the state.
272     * @return derivative of the delay wrt tropospheric model parameters
273     */
274     private double[] rangeRateErrorParameterDerivative(final double[] derivatives, final int freeStateParameters) {
275         // 0                               -> value of the delay
276         // 1 ... freeStateParameters       -> derivatives of the delay wrt state
277         // freeStateParameters + 1 ... n   -> derivatives of the delay wrt tropospheric parameters
278         final int dim = derivatives.length - 1 - freeStateParameters;
279         final double[] rangeError = new double[dim];
280 
281         for (int i = 0; i < dim; i++) {
282             rangeError[i] = derivatives[1 + freeStateParameters + i];
283         }
284 
285         return rangeError;
286     }
287 
288     /** {@inheritDoc} */
289     @Override
290     public List<ParameterDriver> getParametersDrivers() {
291         return tropoModel.getParametersDrivers();
292     }
293 
294     /** {@inheritDoc} */
295     @Override
296     public void modify(final EstimatedMeasurement<RangeRate> estimated) {
297         final RangeRate       measurement = estimated.getObservedMeasurement();
298         final GroundStation   station     = measurement.getStation();
299         final SpacecraftState state       = estimated.getStates()[0];
300 
301         final double[] oldValue = estimated.getEstimatedValue();
302 
303         // update estimated derivatives with Jacobian of the measure wrt state
304         final TroposphericDSConverterTroposphericDSConverter.html#TroposphericDSConverter">TroposphericDSConverter converter = new TroposphericDSConverter(state, 6, Propagator.DEFAULT_LAW);
305         final FieldSpacecraftState<DerivativeStructure> dsState = converter.getState(tropoModel);
306         final DerivativeStructure[] dsParameters = converter.getParameters(dsState, tropoModel);
307         final DerivativeStructure dsDelay = rangeRateErrorTroposphericModel(station, dsState, dsParameters);
308         final double[] derivatives = dsDelay.getAllDerivatives();
309 
310         double[][] djac = new double[1][6];
311         // This implementation will disappear when the implementations of TroposphericModel
312         // will directly be implementations of DiscreteTroposphericModel
313         if (tropoModel instanceof TroposphericModel) {
314             djac = rangeRateErrorJacobianState(station, state);
315         } else {
316             djac = rangeRateErrorJacobianState(derivatives, converter.getFreeStateParameters());
317         }
318         final double[][] stateDerivatives = estimated.getStateDerivatives(0);
319         for (int irow = 0; irow < stateDerivatives.length; ++irow) {
320             for (int jcol = 0; jcol < stateDerivatives[0].length; ++jcol) {
321                 stateDerivatives[irow][jcol] += djac[irow][jcol];
322             }
323         }
324         estimated.setStateDerivatives(0, stateDerivatives);
325 
326         int index = 0;
327         for (final ParameterDriver driver : getParametersDrivers()) {
328             if (driver.isSelected()) {
329                 // update estimated derivatives with derivative of the modification wrt tropospheric parameters
330                 double parameterDerivative = estimated.getParameterDerivatives(driver)[0];
331                 final double[] dDelaydP    = rangeRateErrorParameterDerivative(derivatives, converter.getFreeStateParameters());
332                 parameterDerivative += dDelaydP[index];
333                 estimated.setParameterDerivatives(driver, parameterDerivative);
334                 index += 1;
335             }
336 
337         }
338 
339         for (final ParameterDriver driver : Arrays.asList(station.getClockOffsetDriver(),
340                                                           station.getEastOffsetDriver(),
341                                                           station.getNorthOffsetDriver(),
342                                                           station.getZenithOffsetDriver())) {
343             if (driver.isSelected()) {
344                 // update estimated derivatives with derivative of the modification wrt station parameters
345                 double parameterDerivative = estimated.getParameterDerivatives(driver)[0];
346                 parameterDerivative += rangeRateErrorParameterDerivative(station, driver, state);
347                 estimated.setParameterDerivatives(driver, parameterDerivative);
348             }
349         }
350 
351         // update estimated value taking into account the tropospheric delay.
352         // The tropospheric delay is directly added to the range.
353         final double[] newValue = oldValue.clone();
354         newValue[0] = newValue[0] + dsDelay.getReal();
355         estimated.setEstimatedValue(newValue);
356 
357     }
358 
359 }