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.GroundStation;
  25. import org.orekit.estimation.measurements.ObservedMeasurement;
  26. import org.orekit.propagation.FieldSpacecraftState;
  27. import org.orekit.propagation.SpacecraftState;
  28. import org.orekit.propagation.integration.AbstractGradientConverter;
  29. import org.orekit.utils.Differentiation;
  30. import org.orekit.utils.ParameterDriver;
  31. import org.orekit.utils.ParameterDriversProvider;
  32. import org.orekit.utils.TimeSpanMap.Span;

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

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

  45.     /** Apply a modifier to an estimated measurement.
  46.      * @param <T> type of the measurement
  47.      * @param estimated estimated measurement to modify
  48.      * @param primeStation prime station
  49.      * @param secondStation second station
  50.      * @param modelEffect model effect
  51.      */
  52.     public static <T extends ObservedMeasurement<T>> void modifyWithoutDerivatives(final EstimatedMeasurementBase<T> estimated,
  53.                                                                                    final GroundStation primeStation,
  54.                                                                                    final GroundStation secondStation,
  55.                                                                                    final ParametricModelEffect modelEffect) {

  57.         final SpacecraftState state       = estimated.getStates()[0];
  58.         final double[]        oldValue    = estimated.getEstimatedValue();
  59.         final double          primeDelay  = modelEffect.evaluate(primeStation, state);
  60.         final double          secondDelay = modelEffect.evaluate(secondStation, state);

  62.         // Update estimated value taking into account the ionospheric delay for each downlink.
  63.         // The ionospheric time delay is directly applied to the TDOA.
  64.         final double[] newValue = oldValue.clone();
  65.         newValue[0] += primeDelay;
  66.         newValue[0] -= secondDelay;
  67.         estimated.setEstimatedValue(newValue);
  68.     }

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

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

  90.         // Update estimated derivatives with Jacobian of the measure wrt state
  91.         final FieldSpacecraftState<Gradient> gState = converter.getState(parametricModel);
  92.         final Gradient[] gParameters       = converter.getParameters(gState, parametricModel);
  93.         final Gradient   primeGDelay       = modelEffectGradient.evaluate(primeStation, gState, gParameters);
  94.         final Gradient   secondGDelay      = modelEffectGradient.evaluate(secondStation, gState, gParameters);
  95.         final double[]   primeDerivatives  = primeGDelay.getGradient();
  96.         final double[]   secondDerivatives = secondGDelay.getGradient();

  98.         final double[][] stateDerivatives  = estimated.getStateDerivatives(0);
  99.         for (int jcol = 0; jcol < stateDerivatives[0].length; ++jcol) {
  100.             stateDerivatives[0][jcol] += primeDerivatives[jcol];
  101.             stateDerivatives[0][jcol] -= secondDerivatives[jcol];
  102.         }
  103.         estimated.setStateDerivatives(0, stateDerivatives);

  105.         int index = 0;
  106.         for (final ParameterDriver driver : parametricModel.getParametersDrivers()) {
  107.             if (driver.isSelected()) {
  108.                 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {

  110.                     // update estimated derivatives with derivative of the modification wrt ionospheric parameters
  111.                     double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
  112.                     parameterDerivative += primeDerivatives[index + converter.getFreeStateParameters()];
  113.                     parameterDerivative -= secondDerivatives[index + converter.getFreeStateParameters()];
  114.                     estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
  115.                     index += 1;
  116.                 }
  117.             }

  119.         }

  121.         // Update derivatives with respect to primary station position
  122.         for (final ParameterDriver driver : Arrays.asList(primeStation.getClockOffsetDriver(),
  123.                                                           primeStation.getEastOffsetDriver(),
  124.                                                           primeStation.getNorthOffsetDriver(),
  125.                                                           primeStation.getZenithOffsetDriver())) {
  126.             if (driver.isSelected()) {
  127.                 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {

  129.                     double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
  130.                     parameterDerivative += Differentiation.differentiate((d, t) -> modelEffect.evaluate(primeStation, state),
  131.                                                                      3, 10.0 * driver.getScale()).value(driver, state.getDate());
  132.                     estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
  133.                 }
  134.             }
  135.         }

  137.         // Update derivatives with respect to secondary station position
  138.         for (final ParameterDriver driver : Arrays.asList(secondStation.getClockOffsetDriver(),
  139.                                                           secondStation.getEastOffsetDriver(),
  140.                                                           secondStation.getNorthOffsetDriver(),
  141.                                                           secondStation.getZenithOffsetDriver())) {
  142.             if (driver.isSelected()) {
  143.                 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {

  145.                     double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
  146.                     parameterDerivative -= Differentiation.differentiate((d, t) -> modelEffect.evaluate(secondStation, state),
  147.                                                                      3, 10.0 * driver.getScale()).value(driver, state.getDate());
  148.                     estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
  149.                 }
  150.             }
  151.         }

  153.         // Update estimated value taking into account the ionospheric delay for each downlink.
  154.         // The ionospheric time delay is directly applied to the TDOA.
  155.         final double[] newValue = oldValue.clone();
  156.         newValue[0] += primeGDelay.getReal();
  157.         newValue[0] -= secondGDelay.getReal();
  158.         estimated.setEstimatedValue(newValue);
  159.     }

  161. }
Created with JaCoCo 0.8.11.202310140853