RangeTroposphericDelayModifier.java

  1. /* Copyright 2002-2022 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.modifiers;

  18. import java.util.Arrays;
  19. import java.util.List;

  20. import org.hipparchus.Field;
  21. import org.hipparchus.CalculusFieldElement;
  22. import org.hipparchus.analysis.differentiation.Gradient;
  23. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  24. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  25. import org.orekit.attitudes.InertialProvider;
  26. import org.orekit.estimation.measurements.EstimatedMeasurement;
  27. import org.orekit.estimation.measurements.EstimationModifier;
  28. import org.orekit.estimation.measurements.GroundStation;
  29. import org.orekit.estimation.measurements.Range;
  30. import org.orekit.models.earth.troposphere.DiscreteTroposphericModel;
  31. import org.orekit.propagation.FieldSpacecraftState;
  32. import org.orekit.propagation.SpacecraftState;
  33. import org.orekit.utils.Differentiation;
  34. import org.orekit.utils.ParameterDriver;
  35. import org.orekit.utils.ParameterFunction;

  36. /** Class modifying theoretical range measurement with tropospheric delay.
  37.  * The effect of tropospheric correction on the range is directly computed
  38.  * through the computation of the tropospheric delay.
  39.  *
  40.  * In general, for GNSS, VLBI, ... there is hardly any frequency dependence in the delay.
  41.  * For SLR techniques however, the frequency dependence is sensitive.
  42.  *
  43.  * @author Maxime Journot
  44.  * @author Joris Olympio
  45.  * @since 8.0
  46.  */
  47. public class RangeTroposphericDelayModifier implements EstimationModifier<Range> {

  48.     /** Tropospheric delay model. */
  49.     private final DiscreteTroposphericModel tropoModel;

  50.     /** Constructor.
  51.      *
  52.      * @param model  Tropospheric delay model appropriate for the current range measurement method.
  53.      */
  54.     public RangeTroposphericDelayModifier(final DiscreteTroposphericModel model) {
  55.         tropoModel = model;
  56.     }

  57.     /** Compute the measurement error due to Troposphere.
  58.      * @param station station
  59.      * @param state spacecraft state
  60.      * @return the measurement error due to Troposphere
  61.      */
  62.     private double rangeErrorTroposphericModel(final GroundStation station, final SpacecraftState state) {
  63.         //
  64.         final Vector3D position = state.getPVCoordinates().getPosition();

  65.         // elevation
  66.         final double elevation = station.getBaseFrame().getElevation(position,
  67.                                                                      state.getFrame(),
  68.                                                                      state.getDate());

  69.         // only consider measures above the horizon
  70.         if (elevation > 0) {
  71.             // delay in meters
  72.             final double delay = tropoModel.pathDelay(elevation, station.getBaseFrame().getPoint(), tropoModel.getParameters(), state.getDate());

  73.             return delay;
  74.         }

  75.         return 0;
  76.     }

  77.     /** Compute the measurement error due to Troposphere.
  78.      * @param <T> type of the element
  79.      * @param station station
  80.      * @param state spacecraft state
  81.      * @param parameters tropospheric model parameters
  82.      * @return the measurement error due to Troposphere
  83.      */
  84.     private <T extends CalculusFieldElement<T>> T rangeErrorTroposphericModel(final GroundStation station,
  85.                                                                           final FieldSpacecraftState<T> state,
  86.                                                                           final T[] parameters) {

  87.         // Field
  88.         final Field<T> field = state.getDate().getField();
  89.         final T zero         = field.getZero();

  90.         // satellite elevation
  91.         final FieldVector3D<T> position     = state.getPVCoordinates().getPosition();
  92.         final T elevation                   = station.getBaseFrame().getElevation(position,
  93.                                                                                   state.getFrame(),
  94.                                                                                   state.getDate());


  95.         // only consider measures above the horizon
  96.         if (elevation.getReal() > 0) {
  97.             // delay in meters
  98.             final T delay = tropoModel.pathDelay(elevation, station.getBaseFrame().getPoint(field), parameters, state.getDate());

  99.             return delay;
  100.         }

  101.         return zero;
  102.     }

  103.     /** Compute the Jacobian of the delay term wrt state using
  104.     * automatic differentiation.
  105.     *
  106.     * @param derivatives tropospheric delay derivatives
  107.     *
  108.     * @return Jacobian of the delay wrt state
  109.     */
  110.     private double[][] rangeErrorJacobianState(final double[] derivatives) {
  111.         final double[][] finiteDifferencesJacobian = new double[1][6];
  112.         System.arraycopy(derivatives, 0, finiteDifferencesJacobian[0], 0, 6);
  113.         return finiteDifferencesJacobian;
  114.     }

  115.     /** Compute the derivative of the delay term wrt parameters.
  116.      *
  117.      * @param station ground station
  118.      * @param driver driver for the station offset parameter
  119.      * @param state spacecraft state
  120.      * @return derivative of the delay wrt station offset parameter
  121.      */
  122.     private double rangeErrorParameterDerivative(final GroundStation station,
  123.                                                  final ParameterDriver driver,
  124.                                                  final SpacecraftState state) {

  125.         final ParameterFunction rangeError = new ParameterFunction() {
  126.             /** {@inheritDoc} */
  127.             @Override
  128.             public double value(final ParameterDriver parameterDriver) {
  129.                 return rangeErrorTroposphericModel(station, state);
  130.             }
  131.         };

  132.         final ParameterFunction rangeErrorDerivative =
  133.                         Differentiation.differentiate(rangeError, 3, 10.0 * driver.getScale());

  134.         return rangeErrorDerivative.value(driver);

  135.     }

  136.     /** Compute the derivative of the delay term wrt parameters using
  137.     * automatic differentiation.
  138.     *
  139.     * @param derivatives tropospheric delay derivatives
  140.     * @param freeStateParameters dimension of the state.
  141.     * @return derivative of the delay wrt tropospheric model parameters
  142.     */
  143.     private double[] rangeErrorParameterDerivative(final double[] derivatives, final int freeStateParameters) {
  144.         // 0 ... freeStateParameters - 1 -> derivatives of the delay wrt state
  145.         // freeStateParameters ... n     -> derivatives of the delay wrt tropospheric parameters
  146.         final int dim = derivatives.length - freeStateParameters;
  147.         final double[] rangeError = new double[dim];

  148.         for (int i = 0; i < dim; i++) {
  149.             rangeError[i] = derivatives[freeStateParameters + i];
  150.         }

  151.         return rangeError;
  152.     }

  153.     /** {@inheritDoc} */
  154.     @Override
  155.     public List<ParameterDriver> getParametersDrivers() {
  156.         return tropoModel.getParametersDrivers();
  157.     }

  158.     /** {@inheritDoc} */
  159.     @Override
  160.     public void modify(final EstimatedMeasurement<Range> estimated) {
  161.         final Range           measurement = estimated.getObservedMeasurement();
  162.         final GroundStation   station     = measurement.getStation();
  163.         final SpacecraftState state       = estimated.getStates()[0];

  164.         final double[] oldValue = estimated.getEstimatedValue();

  165.         // update estimated derivatives with Jacobian of the measure wrt state
  166.         final TroposphericGradientConverter converter =
  167.                 new TroposphericGradientConverter(state, 6, new InertialProvider(state.getFrame()));
  168.         final FieldSpacecraftState<Gradient> gState = converter.getState(tropoModel);
  169.         final Gradient[] gParameters = converter.getParameters(gState, tropoModel);
  170.         final Gradient gDelay = rangeErrorTroposphericModel(station, gState, gParameters);
  171.         final double[] derivatives = gDelay.getGradient();

  172.         final double[][] djac = rangeErrorJacobianState(derivatives);

  173.         final double[][] stateDerivatives = estimated.getStateDerivatives(0);
  174.         for (int irow = 0; irow < stateDerivatives.length; ++irow) {
  175.             for (int jcol = 0; jcol < stateDerivatives[0].length; ++jcol) {
  176.                 stateDerivatives[irow][jcol] += djac[irow][jcol];
  177.             }
  178.         }
  179.         estimated.setStateDerivatives(0, stateDerivatives);

  180.         int index = 0;
  181.         for (final ParameterDriver driver : getParametersDrivers()) {
  182.             if (driver.isSelected()) {
  183.                 // update estimated derivatives with derivative of the modification wrt tropospheric parameters
  184.                 double parameterDerivative = estimated.getParameterDerivatives(driver)[0];
  185.                 final double[] dDelaydP    = rangeErrorParameterDerivative(derivatives, converter.getFreeStateParameters());
  186.                 parameterDerivative += dDelaydP[index];
  187.                 estimated.setParameterDerivatives(driver, parameterDerivative);
  188.                 index = index + 1;
  189.             }

  190.         }

  191.         for (final ParameterDriver driver : Arrays.asList(station.getClockOffsetDriver(),
  192.                                                           station.getEastOffsetDriver(),
  193.                                                           station.getNorthOffsetDriver(),
  194.                                                           station.getZenithOffsetDriver())) {
  195.             if (driver.isSelected()) {
  196.                 // update estimated derivatives with derivative of the modification wrt station parameters
  197.                 double parameterDerivative = estimated.getParameterDerivatives(driver)[0];
  198.                 parameterDerivative += rangeErrorParameterDerivative(station, driver, state);
  199.                 estimated.setParameterDerivatives(driver, parameterDerivative);
  200.             }
  201.         }

  202.         // update estimated value taking into account the tropospheric delay.
  203.         // The tropospheric delay is directly added to the range.
  204.         final double[] newValue = oldValue.clone();
  205.         newValue[0] = newValue[0] + gDelay.getReal();
  206.         estimated.setEstimatedValue(newValue);

  207.     }

  208. }