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;
18
19 import java.util.Arrays;
20
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;
34
35 /** Utility class modifying theoretical range measurement.
36 * @author Maxime Journot
37 * @author Joris Olympio
38 * @since 11.2
39 */
40 public class RangeModifierUtil {
41
42 /** Private constructor for utility class.*/
43 private RangeModifierUtil() {
44 // not used
45 }
46
47 /** Apply a modifier to an estimated measurement.
48 * @param <T> type of the measurement
49 * @param estimated estimated measurement to modify
50 * @param station ground station
51 * @param modelEffect model effect
52 * @since 12.0
53 * @deprecated as of 12.1, replaced by {@link #modifyWithoutDerivatives(EstimatedMeasurementBase,
54 * GroundStation, ParametricModelEffect, EstimationModifier)}
55 */
56 @Deprecated
57 public static <T extends ObservedMeasurement<T>> void modifyWithoutDerivatives(final EstimatedMeasurementBase<T> estimated,
58 final GroundStation station,
59 final ParametricModelEffect modelEffect) {
60 modifyWithoutDerivatives(estimated, station, modelEffect, null);
61 }
62
63 /** Apply a modifier to an estimated measurement.
64 * @param <T> type of the measurement
65 * @param estimated estimated measurement to modify
66 * @param station ground station
67 * @param modelEffect model effect
68 * @param modifier applied modifier
69 * @since 12.1
70 */
71 public static <T extends ObservedMeasurement<T>> void modifyWithoutDerivatives(final EstimatedMeasurementBase<T> estimated,
72 final GroundStation station,
73 final ParametricModelEffect modelEffect,
74 final EstimationModifier<T> modifier) {
75
76 final SpacecraftState state = estimated.getStates()[0];
77 final double[] oldValue = estimated.getEstimatedValue();
78
79 // update estimated value taking into account the ionospheric delay.
80 // The ionospheric delay is directly added to the range.
81 final double[] newValue = oldValue.clone();
82 final double delay = modelEffect.evaluate(station, state);
83 newValue[0] = newValue[0] + delay;
84 estimated.modifyEstimatedValue(modifier, newValue);
85
86 }
87
88 /** Apply a modifier to an estimated measurement.
89 * @param <T> type of the measurement
90 * @param estimated estimated measurement to modify
91 * @param station ground station
92 * @param converter gradient converter
93 * @param parametricModel parametric modifier model
94 * @param modelEffect model effect
95 * @param modelEffectGradient model effect gradient
96 * @deprecated as of 12.1, replaced by {@link #modify(EstimatedMeasurement,
97 * ParameterDriversProvider, AbstractGradientConverter, GroundStation,
98 * ParametricModelEffect, ParametricModelEffectGradient, EstimationModifier)}
99 */
100 @Deprecated
101 public static <T extends ObservedMeasurement<T>> void modify(final EstimatedMeasurement<T> estimated,
102 final ParameterDriversProvider parametricModel,
103 final AbstractGradientConverter converter,
104 final GroundStation station,
105 final ParametricModelEffect modelEffect,
106 final ParametricModelEffectGradient modelEffectGradient) {
107 modify(estimated, parametricModel, converter, station, modelEffect, modelEffectGradient, null);
108 }
109
110 /** Apply a modifier to an estimated measurement.
111 * @param <T> type of the measurement
112 * @param estimated estimated measurement to modify
113 * @param station ground 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 */
120 public static <T extends ObservedMeasurement<T>> void modify(final EstimatedMeasurement<T> estimated,
121 final ParameterDriversProvider parametricModel,
122 final AbstractGradientConverter converter,
123 final GroundStation station,
124 final ParametricModelEffect modelEffect,
125 final ParametricModelEffectGradient modelEffectGradient,
126 final EstimationModifier<T> modifier) {
127
128 final SpacecraftState state = estimated.getStates()[0];
129
130 // update estimated derivatives with Jacobian of the measure wrt state
131 final FieldSpacecraftState<Gradient> gState = converter.getState(parametricModel);
132 final Gradient[] gParameters = converter.getParameters(gState, parametricModel);
133 final Gradient gDelay = modelEffectGradient.evaluate(station, gState, gParameters);
134 final double[] derivatives = gDelay.getGradient();
135
136 final double[][] stateDerivatives = estimated.getStateDerivatives(0);
137 for (int jcol = 0; jcol < stateDerivatives[0].length; ++jcol) {
138 stateDerivatives[0][jcol] += derivatives[jcol];
139 }
140 estimated.setStateDerivatives(0, stateDerivatives);
141
142 int index = 0;
143 for (final ParameterDriver driver : parametricModel.getParametersDrivers()) {
144 if (driver.isSelected()) {
145 // update estimated derivatives with derivative of the modification wrt ionospheric parameters
146 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
147 double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
148 parameterDerivative += derivatives[index + converter.getFreeStateParameters()];
149 estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
150 index = index + 1;
151 }
152 }
153
154 }
155
156 for (final ParameterDriver driver : Arrays.asList(station.getClockOffsetDriver(),
157 station.getEastOffsetDriver(),
158 station.getNorthOffsetDriver(),
159 station.getZenithOffsetDriver())) {
160 if (driver.isSelected()) {
161 // update estimated derivatives with derivative of the modification wrt station parameters
162 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
163 double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
164 parameterDerivative += Differentiation.differentiate((d, t) -> modelEffect.evaluate(station, state),
165 3, 10.0 * driver.getScale()).value(driver, state.getDate());
166 estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
167 }
168 }
169 }
170
171 // update estimated value taking into account the ionospheric delay.
172 modifyWithoutDerivatives(estimated, station, modelEffect, modifier);
173
174 }
175
176 }