1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
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
36
37
38
39 class TDOAModifierUtil {
40
41
42 private TDOAModifierUtil() {
43
44 }
45
46
47
48
49
50
51
52
53
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) {
60
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);
65
66
67 final double[] newValue = oldValue.clone();
68 newValue[0] += primeDelay;
69 newValue[0] -= secondDelay;
70 estimated.modifyEstimatedValue(modifier, newValue);
71 }
72
73
74
75
76
77
78
79
80
81
82
83
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) {
92
93 final SpacecraftState state = estimated.getStates()[0];
94 final double[] oldValue = estimated.getEstimatedValue();
95
96
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();
103
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);
110
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()) {
115
116
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 }
124
125 }
126
127
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()) {
134
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 }
142
143
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()) {
150
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 }
158
159
160 final double[] newValue = oldValue.clone();
161 newValue[0] += primeGDelay.getReal();
162 newValue[0] -= secondGDelay.getReal();
163 estimated.modifyEstimatedValue(modifier, newValue);
164 }
165
166 }