TDOAModifierUtil.java

  1. /* Copyright 2002-2025 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.      * @param modifier applied modifier
  53.      * @since 12.1
  54.      */
  55.     public static <T extends ObservedMeasurement<T>> void modifyWithoutDerivatives(final EstimatedMeasurementBase<T> estimated,
  56.                                                                                    final GroundStation primeStation,
  57.                                                                                    final GroundStation secondStation,
  58.                                                                                    final ParametricModelEffect modelEffect,
  59.                                                                                    final EstimationModifier<T> modifier) {

  61.         final SpacecraftState state       = estimated.getStates()[0];
  62.         final double[]        oldValue    = estimated.getEstimatedValue();
  63.         final double          primeDelay  = modelEffect.evaluate(primeStation, state);
  64.         final double          secondDelay = modelEffect.evaluate(secondStation, state);

  66.         // Update estimated value taking into account the delay for each downlink. The time delay is directly applied to the TDOA.
  67.         final double[] newValue = oldValue.clone();
  68.         newValue[0] += primeDelay;
  69.         newValue[0] -= secondDelay;
  70.         estimated.modifyEstimatedValue(modifier, newValue);
  71.     }

  73.     /** Apply a modifier to an estimated measurement.
  74.      * @param <T> type of the measurement
  75.      * @param estimated estimated measurement to modify
  76.      * @param primeStation prime station
  77.      * @param secondStation second station
  78.      * @param converter gradient converter
  79.      * @param parametricModel parametric modifier model
  80.      * @param modelEffect model effect
  81.      * @param modelEffectGradient model effect gradient
  82.      * @param modifier applied modifier
  83.      * @since 12.1
  84.      */
  85.     public static <T extends ObservedMeasurement<T>> void modify(final EstimatedMeasurement<T> estimated,
  86.                                                                  final ParameterDriversProvider parametricModel,
  87.                                                                  final AbstractGradientConverter converter,
  88.                                                                  final GroundStation primeStation, final GroundStation secondStation,
  89.                                                                  final ParametricModelEffect modelEffect,
  90.                                                                  final ParametricModelEffectGradient modelEffectGradient,
  91.                                                                  final EstimationModifier<T> modifier) {

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

  96.         // Update estimated derivatives with Jacobian of the measure wrt state
  97.         final FieldSpacecraftState<Gradient> gState = converter.getState(parametricModel);
  98.         final Gradient[] gParameters       = converter.getParameters(gState, parametricModel);
  99.         final Gradient   primeGDelay       = modelEffectGradient.evaluate(primeStation, gState, gParameters);
  100.         final Gradient   secondGDelay      = modelEffectGradient.evaluate(secondStation, gState, gParameters);
  101.         final double[]   primeDerivatives  = primeGDelay.getGradient();
  102.         final double[]   secondDerivatives = secondGDelay.getGradient();

  104.         final double[][] stateDerivatives  = estimated.getStateDerivatives(0);
  105.         for (int jcol = 0; jcol < stateDerivatives[0].length; ++jcol) {
  106.             stateDerivatives[0][jcol] += primeDerivatives[jcol];
  107.             stateDerivatives[0][jcol] -= secondDerivatives[jcol];
  108.         }
  109.         estimated.setStateDerivatives(0, stateDerivatives);

  111.         int index = 0;
  112.         for (final ParameterDriver driver : parametricModel.getParametersDrivers()) {
  113.             if (driver.isSelected()) {
  114.                 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {

  116.                     // update estimated derivatives with derivative of the modification wrt modifier parameters
  117.                     double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
  118.                     parameterDerivative += primeDerivatives[index + converter.getFreeStateParameters()];
  119.                     parameterDerivative -= secondDerivatives[index + converter.getFreeStateParameters()];
  120.                     estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
  121.                     index += 1;
  122.                 }
  123.             }

  125.         }

  127.         // Update derivatives with respect to primary station position
  128.         for (final ParameterDriver driver : Arrays.asList(primeStation.getClockOffsetDriver(),
  129.                                                           primeStation.getEastOffsetDriver(),
  130.                                                           primeStation.getNorthOffsetDriver(),
  131.                                                           primeStation.getZenithOffsetDriver())) {
  132.             if (driver.isSelected()) {
  133.                 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {

  135.                     double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
  136.                     parameterDerivative += Differentiation.differentiate((d, t) -> modelEffect.evaluate(primeStation, state),
  137.                                                                      3, 10.0 * driver.getScale()).value(driver, state.getDate());
  138.                     estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
  139.                 }
  140.             }
  141.         }

  143.         // Update derivatives with respect to secondary station position
  144.         for (final ParameterDriver driver : Arrays.asList(secondStation.getClockOffsetDriver(),
  145.                                                           secondStation.getEastOffsetDriver(),
  146.                                                           secondStation.getNorthOffsetDriver(),
  147.                                                           secondStation.getZenithOffsetDriver())) {
  148.             if (driver.isSelected()) {
  149.                 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {

  151.                     double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
  152.                     parameterDerivative -= Differentiation.differentiate((d, t) -> modelEffect.evaluate(secondStation, state),
  153.                                                                      3, 10.0 * driver.getScale()).value(driver, state.getDate());
  154.                     estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
  155.                 }
  156.             }
  157.         }

  159.         // Update estimated value taking into account the delay for each downlink.  The time delay is directly applied to the TDOA.
  160.         final double[] newValue = oldValue.clone();
  161.         newValue[0] += primeGDelay.getReal();
  162.         newValue[0] -= secondGDelay.getReal();
  163.         estimated.modifyEstimatedValue(modifier, newValue);
  164.     }

  166. }
Created with JaCoCo 0.8.12.202403310830