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.Range;
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 measurement with tropospheric delay.
44   * The effect of tropospheric correction on the range is directly computed
45   * through the computation of the tropospheric delay.
46   *
47   * In general, for GNSS, VLBI, ... there is hardly any frequency dependence in the delay.
48   * For SLR techniques however, the frequency dependence is sensitive.
49   *
50   * @author Maxime Journot
51   * @author Joris Olympio
52   * @since 8.0
53   */
54  public class RangeTroposphericDelayModifier implements EstimationModifier<Range> {
55  
56      /** Tropospheric delay model. */
57      private final DiscreteTroposphericModel tropoModel;
58  
59      /** Constructor.
60       *
61       * @param model  Tropospheric delay model appropriate for the current range measurement method.
62       */
63      public RangeTroposphericDelayModifier(final DiscreteTroposphericModel model) {
64          tropoModel = model;
65      }
66  
67      /** Get the station height above mean sea level.
68       *
69       * @param station  ground station (or measuring station)
70       * @return the measuring station height above sea level, m
71       */
72      private double getStationHeightAMSL(final GroundStation station) {
73          // FIXME heigth should be computed with respect to geoid WGS84+GUND = EGM2008 for example
74          final double height = station.getBaseFrame().getPoint().getAltitude();
75          return height;
76      }
77  
78      /** Get the station height above mean sea level.
79      * @param <T> type of the elements
80      * @param field field of the elements
81      * @param station  ground station (or measuring station)
82      * @return the measuring station height above sea level, m
83      */
84      private <T extends RealFieldElement<T>> T getStationHeightAMSL(final Field<T> field, final GroundStation station) {
85          // FIXME heigth should be computed with respect to geoid WGS84+GUND = EGM2008 for example
86          final T height = station.getBaseFrame().getPoint(field).getAltitude();
87          return height;
88      }
89  
90      /** Compute the measurement error due to Troposphere.
91       * @param station station
92       * @param state spacecraft state
93       * @return the measurement error due to Troposphere
94       */
95      private double rangeErrorTroposphericModel(final GroundStation station, final SpacecraftState state) {
96          //
97          final Vector3D position = state.getPVCoordinates().getPosition();
98  
99          // elevation
100         final double elevation = station.getBaseFrame().getElevation(position,
101                                                                      state.getFrame(),
102                                                                      state.getDate());
103 
104         // only consider measures above the horizon
105         if (elevation > 0) {
106             // altitude AMSL in meters
107             final double height = getStationHeightAMSL(station);
108 
109             // delay in meters
110             final double delay = tropoModel.pathDelay(elevation, height, tropoModel.getParameters(), state.getDate());
111 
112             return delay;
113         }
114 
115         return 0;
116     }
117 
118     /** Compute the measurement error due to Troposphere.
119      * @param <T> type of the element
120      * @param station station
121      * @param state spacecraft state
122      * @param parameters tropospheric model parameters
123      * @return the measurement error due to Troposphere
124      */
125     private <T extends RealFieldElement<T>> T rangeErrorTroposphericModel(final GroundStation station,
126                                                                           final FieldSpacecraftState<T> state,
127                                                                           final T[] parameters) {
128 
129         // Field
130         final Field<T> field = state.getDate().getField();
131         final T zero         = field.getZero();
132 
133         // satellite elevation
134         final FieldVector3D<T> position     = state.getPVCoordinates().getPosition();
135         final T elevation                   = station.getBaseFrame().getElevation(position,
136                                                                                   state.getFrame(),
137                                                                                   state.getDate());
138 
139 
140         // only consider measures above the horizon
141         if (elevation.getReal() > 0) {
142             // altitude AMSL in meters
143             final T height = getStationHeightAMSL(field, station);
144 
145             // delay in meters
146             final T delay = tropoModel.pathDelay(elevation, height, parameters, state.getDate());
147 
148             return delay;
149         }
150 
151         return zero;
152     }
153 
154     /** Compute the Jacobian of the delay term wrt state using
155     * automatic differentiation.
156     *
157     * @param derivatives tropospheric delay derivatives
158     * @param freeStateParameters dimension of the state.
159     *
160     * @return Jacobian of the delay wrt state
161     */
162     private double[][] rangeErrorJacobianState(final double[] derivatives, final int freeStateParameters) {
163         final double[][] finiteDifferencesJacobian = new double[1][6];
164         for (int i = 0; i < freeStateParameters; i++) {
165             // First element is the value of the delay
166             finiteDifferencesJacobian[0][i] = derivatives[i + 1];
167         }
168         return finiteDifferencesJacobian;
169     }
170 
171     /** Compute the Jacobian of the delay term wrt state.
172     *
173     * @param station station
174     * @param refstate reference spacecraft state
175     *
176     * @return Jacobian of the delay wrt state
177     */
178     private double[][] rangeErrorJacobianState(final GroundStation station, final SpacecraftState refstate) {
179         final double[][] finiteDifferencesJacobian =
180                        Differentiation.differentiate(new StateFunction() {
181                            public double[] value(final SpacecraftState state) {
182                                // evaluate target's elevation with a changed target position
183                                final double value = rangeErrorTroposphericModel(station, state);
184 
185                                return new double[] {value };
186                            }
187                        }, 1, Propagator.DEFAULT_LAW, OrbitType.CARTESIAN,
188                        PositionAngle.TRUE, 15.0, 3).value(refstate);
189 
190         return finiteDifferencesJacobian;
191     }
192 
193     /** Compute the derivative of the delay term wrt parameters.
194      *
195      * @param station ground station
196      * @param driver driver for the station offset parameter
197      * @param state spacecraft state
198      * @return derivative of the delay wrt station offset parameter
199      */
200     private double rangeErrorParameterDerivative(final GroundStation station,
201                                                  final ParameterDriver driver,
202                                                  final SpacecraftState state) {
203 
204         final ParameterFunctionParameterFunction">ParameterFunction rangeError = new ParameterFunction() {
205             /** {@inheritDoc} */
206             @Override
207             public double value(final ParameterDriver parameterDriver) {
208                 return rangeErrorTroposphericModel(station, state);
209             }
210         };
211 
212         final ParameterFunction rangeErrorDerivative =
213                         Differentiation.differentiate(rangeError, 3, 10.0 * driver.getScale());
214 
215         return rangeErrorDerivative.value(driver);
216 
217     }
218 
219     /** Compute the derivative of the delay term wrt parameters using
220     * automatic differentiation.
221     *
222     * @param derivatives tropospheric delay derivatives
223     * @param freeStateParameters dimension of the state.
224     * @return derivative of the delay wrt tropospheric model parameters
225     */
226     private double[] rangeErrorParameterDerivative(final double[] derivatives, final int freeStateParameters) {
227         // 0                               -> value of the delay
228         // 1 ... freeStateParameters       -> derivatives of the delay wrt state
229         // freeStateParameters + 1 ... n   -> derivatives of the delay wrt tropospheric parameters
230         final int dim = derivatives.length - 1 - freeStateParameters;
231         final double[] rangeError = new double[dim];
232 
233         for (int i = 0; i < dim; i++) {
234             rangeError[i] = derivatives[1 + freeStateParameters + i];
235         }
236 
237         return rangeError;
238     }
239 
240     /** {@inheritDoc} */
241     @Override
242     public List<ParameterDriver> getParametersDrivers() {
243         return tropoModel.getParametersDrivers();
244     }
245 
246     /** {@inheritDoc} */
247     @Override
248     public void modify(final EstimatedMeasurement<Range> estimated) {
249         final Range           measurement = estimated.getObservedMeasurement();
250         final GroundStation   station     = measurement.getStation();
251         final SpacecraftState state       = estimated.getStates()[0];
252 
253         final double[] oldValue = estimated.getEstimatedValue();
254 
255         // update estimated derivatives with Jacobian of the measure wrt state
256         final TroposphericDSConverterTroposphericDSConverter.html#TroposphericDSConverter">TroposphericDSConverter converter = new TroposphericDSConverter(state, 6, Propagator.DEFAULT_LAW);
257         final FieldSpacecraftState<DerivativeStructure> dsState = converter.getState(tropoModel);
258         final DerivativeStructure[] dsParameters = converter.getParameters(dsState, tropoModel);
259         final DerivativeStructure dsDelay = rangeErrorTroposphericModel(station, dsState, dsParameters);
260         final double[] derivatives = dsDelay.getAllDerivatives();
261 
262         double[][] djac = new double[1][6];
263         // This implementation will disappear when the implementations of TroposphericModel
264         // will directly be implementations of DiscreteTroposphericModel
265         if (tropoModel instanceof TroposphericModel) {
266             djac = rangeErrorJacobianState(station, state);
267         } else {
268             djac = rangeErrorJacobianState(derivatives, converter.getFreeStateParameters());
269         }
270 
271         final double[][] stateDerivatives = estimated.getStateDerivatives(0);
272         for (int irow = 0; irow < stateDerivatives.length; ++irow) {
273             for (int jcol = 0; jcol < stateDerivatives[0].length; ++jcol) {
274                 stateDerivatives[irow][jcol] += djac[irow][jcol];
275             }
276         }
277         estimated.setStateDerivatives(0, stateDerivatives);
278 
279         int index = 0;
280         for (final ParameterDriver driver : getParametersDrivers()) {
281             if (driver.isSelected()) {
282                 // update estimated derivatives with derivative of the modification wrt tropospheric parameters
283                 double parameterDerivative = estimated.getParameterDerivatives(driver)[0];
284                 final double[] dDelaydP    = rangeErrorParameterDerivative(derivatives, converter.getFreeStateParameters());
285                 parameterDerivative += dDelaydP[index];
286                 estimated.setParameterDerivatives(driver, parameterDerivative);
287                 index = index + 1;
288             }
289 
290         }
291 
292         for (final ParameterDriver driver : Arrays.asList(station.getClockOffsetDriver(),
293                                                           station.getEastOffsetDriver(),
294                                                           station.getNorthOffsetDriver(),
295                                                           station.getZenithOffsetDriver())) {
296             if (driver.isSelected()) {
297                 // update estimated derivatives with derivative of the modification wrt station parameters
298                 double parameterDerivative = estimated.getParameterDerivatives(driver)[0];
299                 parameterDerivative += rangeErrorParameterDerivative(station, driver, state);
300                 estimated.setParameterDerivatives(driver, parameterDerivative);
301             }
302         }
303 
304         // update estimated value taking into account the tropospheric delay.
305         // The tropospheric delay is directly added to the range.
306         final double[] newValue = oldValue.clone();
307         newValue[0] = newValue[0] + dsDelay.getReal();
308         estimated.setEstimatedValue(newValue);
309 
310     }
311 
312 }