BistaticModifierUtil.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 bistatic measurements.
  36.  * @author Pascal Parraud
  37.  * @since 11.2
  38.  */
  39. class BistaticModifierUtil {

  41.     /** Private constructor for utility class.*/
  42.     private BistaticModifierUtil() {
  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 emitter emitter station
  50.      * @param receiver receiver station
  51.      * @param modelEffect model effect
  52.      * @deprecated as of 12.1, replaced by {@link #modify(EstimatedMeasurementBase,
  53.      * GroundStation, GroundStation, ParametricModelEffect, EstimationModifier)}
  54.      */
  55.     @Deprecated
  56.     public static <T extends ObservedMeasurement<T>> void modify(final EstimatedMeasurementBase<T> estimated,
  57.                                                                  final GroundStation emitter, final GroundStation receiver,
  58.                                                                  final ParametricModelEffect modelEffect) {
  59.         modify(estimated, emitter, receiver, modelEffect, null);
  60.     }

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

  76.         // update estimated value taking into account the model effect.
  77.         // The model effect delay is directly added to the measurement.
  78.         final SpacecraftState state    = estimated.getStates()[0];
  79.         final double[]        newValue = estimated.getEstimatedValue().clone();
  80.         newValue[0] += modelEffect.evaluate(emitter, state);
  81.         newValue[0] += modelEffect.evaluate(receiver, state);
  82.         estimated.modifyEstimatedValue(modifier, newValue);

  84.     }

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

  109.     /** Apply a modifier to an estimated measurement.
  110.      * @param <T> type of the measurement
  111.      * @param estimated estimated measurement to modify
  112.      * @param emitter emitter station
  113.      * @param receiver receiver station
  114.      * @param converter gradient converter
  115.      * @param parametricModel parametric modifier model
  116.      * @param modelEffect model effect
  117.      * @param modelEffectGradient model effect gradient
  118.      * @param modifier applied modifier
  119.      * @since 12.1
  120.      */
  121.     public static <T extends ObservedMeasurement<T>> void modify(final EstimatedMeasurement<T> estimated,
  122.                                                                  final ParameterDriversProvider parametricModel,
  123.                                                                  final AbstractGradientConverter converter,
  124.                                                                  final GroundStation emitter, final GroundStation receiver,
  125.                                                                  final ParametricModelEffect modelEffect,
  126.                                                                  final ParametricModelEffectGradient modelEffectGradient,
  127.                                                                  final EstimationModifier<T> modifier) {

  129.         final SpacecraftState state    = estimated.getStates()[0];

  131.         // update estimated derivatives with Jacobian of the measure wrt state
  132.         final FieldSpacecraftState<Gradient> gState = converter.getState(parametricModel);
  133.         final Gradient[] gParameters = converter.getParameters(gState, parametricModel);

  135.         final Gradient delayUp = modelEffectGradient.evaluate(emitter, gState, gParameters);
  136.         final double[] derivativesUp = delayUp.getGradient();

  138.         final Gradient delayDown = modelEffectGradient.evaluate(receiver, gState, gParameters);
  139.         final double[] derivativesDown = delayDown.getGradient();

  141.         // update estimated derivatives with Jacobian of the measure wrt state
  142.         final double[][] stateDerivatives = estimated.getStateDerivatives(0);
  143.         for (int jcol = 0; jcol < stateDerivatives[0].length; ++jcol) {
  144.             stateDerivatives[0][jcol] += derivativesUp[jcol];
  145.             stateDerivatives[0][jcol] += derivativesDown[jcol];
  146.         }
  147.         estimated.setStateDerivatives(0, stateDerivatives);

  149.         int index = 0;
  150.         for (final ParameterDriver driver : parametricModel.getParametersDrivers()) {
  151.             if (driver.isSelected()) {
  152.                 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {

  154.                     // update estimated derivatives with derivative of the modification wrt model parameters
  155.                     double parameterDerivative  = estimated.getParameterDerivatives(driver, span.getStart())[0];
  156.                     parameterDerivative += derivativesUp[index + converter.getFreeStateParameters()];
  157.                     parameterDerivative += derivativesDown[index + converter.getFreeStateParameters()];
  158.                     estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
  159.                     index++;
  160.                 }
  161.             }

  163.         }

  165.         for (final ParameterDriver driver : Arrays.asList(emitter.getEastOffsetDriver(),
  166.                                                           emitter.getNorthOffsetDriver(),
  167.                                                           emitter.getZenithOffsetDriver())) {
  168.             if (driver.isSelected()) {
  169.                 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {

  171.                     // update estimated derivatives with derivative of the modification wrt station parameters
  172.                     double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
  173.                     parameterDerivative += Differentiation.differentiate((d, t) -> modelEffect.evaluate(emitter, state),
  174.                                                                          3, 10.0 * driver.getScale()).value(driver, state.getDate());
  175.                     estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
  176.                 }
  177.             }
  178.         }

  180.         for (final ParameterDriver driver : Arrays.asList(receiver.getClockOffsetDriver(),
  181.                                                           receiver.getEastOffsetDriver(),
  182.                                                           receiver.getNorthOffsetDriver(),
  183.                                                           receiver.getZenithOffsetDriver())) {
  184.             if (driver.isSelected()) {
  185.                 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {

  187.                     // update estimated derivatives with derivative of the modification wrt station parameters
  188.                     double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
  189.                     parameterDerivative += Differentiation.differentiate((d, t) -> modelEffect.evaluate(receiver, state),
  190.                                                                          3, 10.0 * driver.getScale()).value(driver, state.getDate());
  191.                     estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
  192.                 }
  193.             }
  194.         }

  196.         // modify the value
  197.         modify(estimated, emitter, receiver, modelEffect, modifier);

  199.     }

  201. }
Created with JaCoCo 0.8.12.202403310830