TurnAroundRangeTroposphericDelayModifier.java

  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.modifiers;

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

  20. import org.hipparchus.Field;
  21. import org.hipparchus.RealFieldElement;
  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.TurnAroundRange;
  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 turn-around TurnAroundRange measurement with tropospheric delay.
  37.  * The effect of tropospheric correction on the TurnAroundRange 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.  * @since 9.0
  45.  */
  46. public class TurnAroundRangeTroposphericDelayModifier implements EstimationModifier<TurnAroundRange> {

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

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

  56.     /** Get the station height above mean sea level.
  57.      *
  58.      * @param station  ground station (or measuring station)
  59.      * @return the measuring station height above sea level, m
  60.      */
  61.     private double getStationHeightAMSL(final GroundStation station) {
  62.         // FIXME height should be computed with respect to geoid WGS84+GUND = EGM2008 for example
  63.         final double height = station.getBaseFrame().getPoint().getAltitude();
  64.         return height;
  65.     }

  66.     /** Get the station height above mean sea level.
  67.     * @param <T> type of the elements
  68.     * @param field field of the elements
  69.     * @param station  ground station (or measuring station)
  70.     * @return the measuring station height above sea level, m
  71.     */
  72.     private <T extends RealFieldElement<T>> T getStationHeightAMSL(final Field<T> field,
  73.                                                                    final GroundStation station) {
  74.         // FIXME heigth should be computed with respect to geoid WGS84+GUND = EGM2008 for example
  75.         final T height = station.getBaseFrame().getPoint(field).getAltitude();
  76.         return height;
  77.     }

  78.     /** Compute the measurement error due to Troposphere.
  79.      * @param station station
  80.      * @param state spacecraft state
  81.      * @return the measurement error due to Troposphere
  82.      */
  83.     private double rangeErrorTroposphericModel(final GroundStation station, final SpacecraftState state) {
  84.         //
  85.         final Vector3D position = state.getPVCoordinates().getPosition();

  86.         // elevation
  87.         final double elevation = station.getBaseFrame().getElevation(position,
  88.                                                                      state.getFrame(),
  89.                                                                      state.getDate());

  90.         // only consider measures above the horizon
  91.         if (elevation > 0) {
  92.             // altitude AMSL in meters
  93.             final double height = getStationHeightAMSL(station);

  94.             // Delay in meters
  95.             final double delay = tropoModel.pathDelay(elevation, height, tropoModel.getParameters(), state.getDate());

  96.             return delay;
  97.         }

  98.         return 0;
  99.     }

  100.     /** Compute the measurement error due to Troposphere.
  101.      * @param <T> type of the element
  102.      * @param station station
  103.      * @param state spacecraft state
  104.      * @param parameters tropospheric model parameters
  105.      * @return the measurement error due to Troposphere
  106.      */
  107.     private <T extends RealFieldElement<T>> T rangeErrorTroposphericModel(final GroundStation station,
  108.                                                                           final FieldSpacecraftState<T> state,
  109.                                                                           final T[] parameters) {
  110.         // Field
  111.         final Field<T> field = state.getDate().getField();
  112.         final T zero         = field.getZero();

  113.         //
  114.         final FieldVector3D<T> position = state.getPVCoordinates().getPosition();
  115.         final T dsElevation             = station.getBaseFrame().getElevation(position,
  116.                                                                               state.getFrame(),
  117.                                                                               state.getDate());

  118.         // only consider measures above the horizon
  119.         if (dsElevation.getReal() > 0) {
  120.             // altitude AMSL in meters
  121.             final T height = getStationHeightAMSL(field, station);

  122.             // Delay in meters
  123.             final T delay = tropoModel.pathDelay(dsElevation, height, parameters, state.getDate());

  124.             return delay;
  125.         }

  126.         return zero;
  127.     }

  128.     /** Compute the Jacobian of the delay term wrt state using
  129.     * automatic differentiation.
  130.     *
  131.     * @param derivatives tropospheric delay derivatives
  132.     *
  133.     * @return Jacobian of the delay wrt state
  134.     */
  135.     private double[][] rangeErrorJacobianState(final double[] derivatives) {
  136.         final double[][] finiteDifferencesJacobian = new double[1][6];
  137.         System.arraycopy(derivatives, 0, finiteDifferencesJacobian[0], 0, 6);
  138.         return finiteDifferencesJacobian;
  139.     }


  140.     /** Compute the derivative of the delay term wrt parameters.
  141.      *
  142.      * @param station ground station
  143.      * @param driver driver for the station offset parameter
  144.      * @param state spacecraft state
  145.      * @return derivative of the delay wrt station offset parameter
  146.      */
  147.     private double rangeErrorParameterDerivative(final GroundStation station,
  148.                                                  final ParameterDriver driver,
  149.                                                  final SpacecraftState state) {

  150.         final ParameterFunction rangeError = new ParameterFunction() {
  151.             /** {@inheritDoc} */
  152.             @Override
  153.             public double value(final ParameterDriver parameterDriver) {
  154.                 return rangeErrorTroposphericModel(station, state);
  155.             }
  156.         };

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

  158.         return rangeErrorDerivative.value(driver);

  159.     }

  160.     /** Compute the derivative of the delay term wrt parameters using
  161.     * automatic differentiation.
  162.     *
  163.     * @param derivatives tropospheric delay derivatives
  164.     * @param freeStateParameters dimension of the state.
  165.     * @return derivative of the delay wrt tropospheric model parameters
  166.     */
  167.     private double[] rangeErrorParameterDerivative(final double[] derivatives, final int freeStateParameters) {
  168.         // 0 ... freeStateParameters - 1 -> derivatives of the delay wrt state
  169.         // freeStateParameters ... n     -> derivatives of the delay wrt tropospheric parameters
  170.         final int dim = derivatives.length - freeStateParameters;
  171.         final double[] rangeError = new double[dim];

  172.         for (int i = 0; i < dim; i++) {
  173.             rangeError[i] = derivatives[freeStateParameters + i];
  174.         }

  175.         return rangeError;
  176.     }

  177.     /** {@inheritDoc} */
  178.     @Override
  179.     public List<ParameterDriver> getParametersDrivers() {
  180.         return tropoModel.getParametersDrivers();
  181.     }

  182.     /** {@inheritDoc} */
  183.     @Override
  184.     public void modify(final EstimatedMeasurement<TurnAroundRange> estimated) {
  185.         final TurnAroundRange measurement   = estimated.getObservedMeasurement();
  186.         final GroundStation   masterStation = measurement.getMasterStation();
  187.         final GroundStation   slaveStation  = measurement.getSlaveStation();
  188.         final SpacecraftState state         = estimated.getStates()[0];

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

  190.         // Update estimated derivatives with Jacobian of the measure wrt state
  191.         final TroposphericGradientConverter converter =
  192.                 new TroposphericGradientConverter(state, 6, new InertialProvider(state.getFrame()));
  193.         final FieldSpacecraftState<Gradient> gState = converter.getState(tropoModel);
  194.         final Gradient[] gParameters = converter.getParameters(gState, tropoModel);
  195.         final Gradient masterGDelay = rangeErrorTroposphericModel(masterStation, gState, gParameters);
  196.         final Gradient slaveGDelay = rangeErrorTroposphericModel(slaveStation, gState, gParameters);
  197.         final double[] masterDerivatives = masterGDelay.getGradient();
  198.         final double[] slaveDerivatives  = masterGDelay.getGradient();

  199.         final double[][] masterDjac = rangeErrorJacobianState(masterDerivatives);
  200.         final double[][] slaveDjac  = rangeErrorJacobianState(slaveDerivatives);
  201.         final double[][] stateDerivatives = estimated.getStateDerivatives(0);
  202.         for (int irow = 0; irow < stateDerivatives.length; ++irow) {
  203.             for (int jcol = 0; jcol < stateDerivatives[0].length; ++jcol) {
  204.                 stateDerivatives[irow][jcol] += masterDjac[irow][jcol] + slaveDjac[irow][jcol];
  205.             }
  206.         }
  207.         estimated.setStateDerivatives(0, stateDerivatives);

  208.         int indexMaster = 0;
  209.         for (final ParameterDriver driver : getParametersDrivers()) {
  210.             if (driver.isSelected()) {
  211.                 // update estimated derivatives with derivative of the modification wrt tropospheric parameters
  212.                 double parameterDerivative = estimated.getParameterDerivatives(driver)[0];
  213.                 final double[] derivatives = rangeErrorParameterDerivative(masterDerivatives, converter.getFreeStateParameters());
  214.                 parameterDerivative += derivatives[indexMaster];
  215.                 estimated.setParameterDerivatives(driver, parameterDerivative);
  216.                 indexMaster += 1;
  217.             }

  218.         }

  219.         int indexSlave = 0;
  220.         for (final ParameterDriver driver : getParametersDrivers()) {
  221.             if (driver.isSelected()) {
  222.                 // update estimated derivatives with derivative of the modification wrt tropospheric parameters
  223.                 double parameterDerivative = estimated.getParameterDerivatives(driver)[0];
  224.                 final double[] derivatives = rangeErrorParameterDerivative(slaveDerivatives, converter.getFreeStateParameters());
  225.                 parameterDerivative += derivatives[indexSlave];
  226.                 estimated.setParameterDerivatives(driver, parameterDerivative);
  227.                 indexSlave += 1;
  228.             }

  229.         }

  230.         // Update derivatives with respect to master station position
  231.         for (final ParameterDriver driver : Arrays.asList(masterStation.getClockOffsetDriver(),
  232.                                                           masterStation.getEastOffsetDriver(),
  233.                                                           masterStation.getNorthOffsetDriver(),
  234.                                                           masterStation.getZenithOffsetDriver())) {
  235.             if (driver.isSelected()) {
  236.                 double parameterDerivative = estimated.getParameterDerivatives(driver)[0];
  237.                 parameterDerivative += rangeErrorParameterDerivative(masterStation, driver, state);
  238.                 estimated.setParameterDerivatives(driver, parameterDerivative);
  239.             }
  240.         }

  241.         // Update derivatives with respect to slave station position
  242.         for (final ParameterDriver driver : Arrays.asList(slaveStation.getEastOffsetDriver(),
  243.                                                           slaveStation.getNorthOffsetDriver(),
  244.                                                           slaveStation.getZenithOffsetDriver())) {
  245.             if (driver.isSelected()) {
  246.                 double parameterDerivative = estimated.getParameterDerivatives(driver)[0];
  247.                 parameterDerivative += rangeErrorParameterDerivative(slaveStation, driver, state);
  248.                 estimated.setParameterDerivatives(driver, parameterDerivative);
  249.             }
  250.         }

  251.         // Update estimated value taking into account the tropospheric delay.
  252.         // The tropospheric delay is directly added to the TurnAroundRange.
  253.         final double[] newValue = oldValue.clone();
  254.         newValue[0] = newValue[0] + masterGDelay.getReal() + slaveGDelay.getReal();
  255.         estimated.setEstimatedValue(newValue);

  256.     }

  257. }