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