TDOAModifierUtil.java

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

  19. import java.util.Arrays;

  21. import org.hipparchus.analysis.differentiation.Gradient;
  22. import org.orekit.estimation.measurements.EstimatedMeasurement;
  23. import org.orekit.estimation.measurements.EstimatedMeasurementBase;
  24. import org.orekit.estimation.measurements.EstimationModifier;
  25. import org.orekit.estimation.measurements.GroundStation;
  26. import org.orekit.estimation.measurements.ObservedMeasurement;
  27. import org.orekit.propagation.FieldSpacecraftState;
  28. import org.orekit.propagation.SpacecraftState;
  29. import org.orekit.propagation.integration.AbstractGradientConverter;
  30. import org.orekit.utils.Differentiation;
  31. import org.orekit.utils.ParameterDriver;
  32. import org.orekit.utils.ParameterDriversProvider;
  33. import org.orekit.utils.TimeSpanMap.Span;

  35. /** Utility class for TDOA measurements.
  36.  * @author Pascal Parraud
  37.  * @since 11.2
  38.  */
  39. class TDOAModifierUtil {

  41.     /** Private constructor for utility class.*/
  42.     private TDOAModifierUtil() {
  43.         // not used
  44.     }

  46.     /** Apply a modifier to an estimated measurement.
  47.      * @param <T> type of the measurement
  48.      * @param estimated estimated measurement to modify
  49.      * @param primeStation prime station
  50.      * @param secondStation second station
  51.      * @param modelEffect model effect
  52.      * @deprecated as of 12.1, replaced by {@link #modifyWithoutDerivatives(EstimatedMeasurementBase,
  53.      * GroundStation, GroundStation, ParametricModelEffect, EstimationModifier)}
  54.      */
  55.     @Deprecated
  56.     public static <T extends ObservedMeasurement<T>> void modifyWithoutDerivatives(final EstimatedMeasurementBase<T> estimated,
  57.                                                                                    final GroundStation primeStation,
  58.                                                                                    final GroundStation secondStation,
  59.                                                                                    final ParametricModelEffect modelEffect) {
  60.         modifyWithoutDerivatives(estimated, primeStation, secondStation, modelEffect, null);
  61.     }

  63.     /** Apply a modifier to an estimated measurement.
  64.      * @param <T> type of the measurement
  65.      * @param estimated estimated measurement to modify
  66.      * @param primeStation prime station
  67.      * @param secondStation second station
  68.      * @param modelEffect model effect
  69.      * @param modifier applied modifier
  70.      * @since 12.1
  71.      */
  72.     public static <T extends ObservedMeasurement<T>> void modifyWithoutDerivatives(final EstimatedMeasurementBase<T> estimated,
  73.                                                                                    final GroundStation primeStation,
  74.                                                                                    final GroundStation secondStation,
  75.                                                                                    final ParametricModelEffect modelEffect,
  76.                                                                                    final EstimationModifier<T> modifier) {

  78.         final SpacecraftState state       = estimated.getStates()[0];
  79.         final double[]        oldValue    = estimated.getEstimatedValue();
  80.         final double          primeDelay  = modelEffect.evaluate(primeStation, state);
  81.         final double          secondDelay = modelEffect.evaluate(secondStation, state);

  83.         // Update estimated value taking into account the ionospheric delay for each downlink.
  84.         // The ionospheric time delay is directly applied to the TDOA.
  85.         final double[] newValue = oldValue.clone();
  86.         newValue[0] += primeDelay;
  87.         newValue[0] -= secondDelay;
  88.         estimated.modifyEstimatedValue(modifier, newValue);
  89.     }

  91.     /** Apply a modifier to an estimated measurement.
  92.      * @param <T> type of the measurement
  93.      * @param estimated estimated measurement to modify
  94.      * @param primeStation prime station
  95.      * @param secondStation second station
  96.      * @param converter gradient converter
  97.      * @param parametricModel parametric modifier model
  98.      * @param modelEffect model effect
  99.      * @param modelEffectGradient model effect gradient
  100.      * @deprecated as of 12.1, replaced by {@link #modify(EstimatedMeasurement,
  101.      * ParameterDriversProvider, AbstractGradientConverter, GroundStation, GroundStation,
  102.      * ParametricModelEffect, ParametricModelEffectGradient, EstimationModifier)}
  103.      */
  104.     @Deprecated
  105.     public static <T extends ObservedMeasurement<T>> void modify(final EstimatedMeasurement<T> estimated,
  106.                                                                  final ParameterDriversProvider parametricModel,
  107.                                                                  final AbstractGradientConverter converter,
  108.                                                                  final GroundStation primeStation, final GroundStation secondStation,
  109.                                                                  final ParametricModelEffect modelEffect,
  110.                                                                  final ParametricModelEffectGradient modelEffectGradient) {
  111.         modify(estimated, parametricModel, converter, primeStation, secondStation,
  112.                modelEffect, modelEffectGradient, null);
  113.     }

  115.     /** Apply a modifier to an estimated measurement.
  116.      * @param <T> type of the measurement
  117.      * @param estimated estimated measurement to modify
  118.      * @param primeStation prime station
  119.      * @param secondStation second station
  120.      * @param converter gradient converter
  121.      * @param parametricModel parametric modifier model
  122.      * @param modelEffect model effect
  123.      * @param modelEffectGradient model effect gradient
  124.      * @param modifier applied modifier
  125.      * @since 12.1
  126.      */
  127.     public static <T extends ObservedMeasurement<T>> void modify(final EstimatedMeasurement<T> estimated,
  128.                                                                  final ParameterDriversProvider parametricModel,
  129.                                                                  final AbstractGradientConverter converter,
  130.                                                                  final GroundStation primeStation, final GroundStation secondStation,
  131.                                                                  final ParametricModelEffect modelEffect,
  132.                                                                  final ParametricModelEffectGradient modelEffectGradient,
  133.                                                                  final EstimationModifier<T> modifier) {

  135.         final SpacecraftState state    = estimated.getStates()[0];
  136.         final double[]        oldValue = estimated.getEstimatedValue();

  138.         // Update estimated derivatives with Jacobian of the measure wrt state
  139.         final FieldSpacecraftState<Gradient> gState = converter.getState(parametricModel);
  140.         final Gradient[] gParameters       = converter.getParameters(gState, parametricModel);
  141.         final Gradient   primeGDelay       = modelEffectGradient.evaluate(primeStation, gState, gParameters);
  142.         final Gradient   secondGDelay      = modelEffectGradient.evaluate(secondStation, gState, gParameters);
  143.         final double[]   primeDerivatives  = primeGDelay.getGradient();
  144.         final double[]   secondDerivatives = secondGDelay.getGradient();

  146.         final double[][] stateDerivatives  = estimated.getStateDerivatives(0);
  147.         for (int jcol = 0; jcol < stateDerivatives[0].length; ++jcol) {
  148.             stateDerivatives[0][jcol] += primeDerivatives[jcol];
  149.             stateDerivatives[0][jcol] -= secondDerivatives[jcol];
  150.         }
  151.         estimated.setStateDerivatives(0, stateDerivatives);

  153.         int index = 0;
  154.         for (final ParameterDriver driver : parametricModel.getParametersDrivers()) {
  155.             if (driver.isSelected()) {
  156.                 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {

  158.                     // update estimated derivatives with derivative of the modification wrt ionospheric parameters
  159.                     double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
  160.                     parameterDerivative += primeDerivatives[index + converter.getFreeStateParameters()];
  161.                     parameterDerivative -= secondDerivatives[index + converter.getFreeStateParameters()];
  162.                     estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
  163.                     index += 1;
  164.                 }
  165.             }

  167.         }

  169.         // Update derivatives with respect to primary station position
  170.         for (final ParameterDriver driver : Arrays.asList(primeStation.getClockOffsetDriver(),
  171.                                                           primeStation.getEastOffsetDriver(),
  172.                                                           primeStation.getNorthOffsetDriver(),
  173.                                                           primeStation.getZenithOffsetDriver())) {
  174.             if (driver.isSelected()) {
  175.                 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {

  177.                     double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
  178.                     parameterDerivative += Differentiation.differentiate((d, t) -> modelEffect.evaluate(primeStation, state),
  179.                                                                      3, 10.0 * driver.getScale()).value(driver, state.getDate());
  180.                     estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
  181.                 }
  182.             }
  183.         }

  185.         // Update derivatives with respect to secondary station position
  186.         for (final ParameterDriver driver : Arrays.asList(secondStation.getClockOffsetDriver(),
  187.                                                           secondStation.getEastOffsetDriver(),
  188.                                                           secondStation.getNorthOffsetDriver(),
  189.                                                           secondStation.getZenithOffsetDriver())) {
  190.             if (driver.isSelected()) {
  191.                 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {

  193.                     double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
  194.                     parameterDerivative -= Differentiation.differentiate((d, t) -> modelEffect.evaluate(secondStation, state),
  195.                                                                      3, 10.0 * driver.getScale()).value(driver, state.getDate());
  196.                     estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
  197.                 }
  198.             }
  199.         }

  201.         // Update estimated value taking into account the ionospheric delay for each downlink.
  202.         // The ionospheric time delay is directly applied to the TDOA.
  203.         final double[] newValue = oldValue.clone();
  204.         newValue[0] += primeGDelay.getReal();
  205.         newValue[0] -= secondGDelay.getReal();
  206.         estimated.modifyEstimatedValue(modifier, newValue);
  207.     }

  209. }
Created with JaCoCo 0.8.12.202403310830