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 import java.util.List;
21
22 import org.hipparchus.Field;
23 import org.hipparchus.RealFieldElement;
24 import org.hipparchus.analysis.differentiation.DerivativeStructure;
25 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
26 import org.hipparchus.geometry.euclidean.threed.Vector3D;
27 import org.orekit.estimation.measurements.EstimatedMeasurement;
28 import org.orekit.estimation.measurements.EstimationModifier;
29 import org.orekit.estimation.measurements.GroundStation;
30 import org.orekit.estimation.measurements.Range;
31 import org.orekit.models.earth.DiscreteTroposphericModel;
32 import org.orekit.models.earth.TroposphericModel;
33 import org.orekit.orbits.OrbitType;
34 import org.orekit.orbits.PositionAngle;
35 import org.orekit.propagation.FieldSpacecraftState;
36 import org.orekit.propagation.Propagator;
37 import org.orekit.propagation.SpacecraftState;
38 import org.orekit.utils.Differentiation;
39 import org.orekit.utils.ParameterDriver;
40 import org.orekit.utils.ParameterFunction;
41 import org.orekit.utils.StateFunction;
42
43
44
45
46
47
48
49
50
51
52
53
54 public class RangeTroposphericDelayModifier implements EstimationModifier<Range> {
55
56
57 private final DiscreteTroposphericModel tropoModel;
58
59
60
61
62
63 public RangeTroposphericDelayModifier(final DiscreteTroposphericModel model) {
64 tropoModel = model;
65 }
66
67
68
69
70
71
72 private double getStationHeightAMSL(final GroundStation station) {
73
74 final double height = station.getBaseFrame().getPoint().getAltitude();
75 return height;
76 }
77
78
79
80
81
82
83
84 private <T extends RealFieldElement<T>> T getStationHeightAMSL(final Field<T> field, final GroundStation station) {
85
86 final T height = station.getBaseFrame().getPoint(field).getAltitude();
87 return height;
88 }
89
90
91
92
93
94
95 private double rangeErrorTroposphericModel(final GroundStation station, final SpacecraftState state) {
96
97 final Vector3D position = state.getPVCoordinates().getPosition();
98
99
100 final double elevation = station.getBaseFrame().getElevation(position,
101 state.getFrame(),
102 state.getDate());
103
104
105 if (elevation > 0) {
106
107 final double height = getStationHeightAMSL(station);
108
109
110 final double delay = tropoModel.pathDelay(elevation, height, tropoModel.getParameters(), state.getDate());
111
112 return delay;
113 }
114
115 return 0;
116 }
117
118
119
120
121
122
123
124
125 private <T extends RealFieldElement<T>> T rangeErrorTroposphericModel(final GroundStation station,
126 final FieldSpacecraftState<T> state,
127 final T[] parameters) {
128
129
130 final Field<T> field = state.getDate().getField();
131 final T zero = field.getZero();
132
133
134 final FieldVector3D<T> position = state.getPVCoordinates().getPosition();
135 final T elevation = station.getBaseFrame().getElevation(position,
136 state.getFrame(),
137 state.getDate());
138
139
140
141 if (elevation.getReal() > 0) {
142
143 final T height = getStationHeightAMSL(field, station);
144
145
146 final T delay = tropoModel.pathDelay(elevation, height, parameters, state.getDate());
147
148 return delay;
149 }
150
151 return zero;
152 }
153
154
155
156
157
158
159
160
161
162 private double[][] rangeErrorJacobianState(final double[] derivatives, final int freeStateParameters) {
163 final double[][] finiteDifferencesJacobian = new double[1][6];
164 for (int i = 0; i < freeStateParameters; i++) {
165
166 finiteDifferencesJacobian[0][i] = derivatives[i + 1];
167 }
168 return finiteDifferencesJacobian;
169 }
170
171
172
173
174
175
176
177
178 private double[][] rangeErrorJacobianState(final GroundStation station, final SpacecraftState refstate) {
179 final double[][] finiteDifferencesJacobian =
180 Differentiation.differentiate(new StateFunction() {
181 public double[] value(final SpacecraftState state) {
182
183 final double value = rangeErrorTroposphericModel(station, state);
184
185 return new double[] {value };
186 }
187 }, 1, Propagator.DEFAULT_LAW, OrbitType.CARTESIAN,
188 PositionAngle.TRUE, 15.0, 3).value(refstate);
189
190 return finiteDifferencesJacobian;
191 }
192
193
194
195
196
197
198
199
200 private double rangeErrorParameterDerivative(final GroundStation station,
201 final ParameterDriver driver,
202 final SpacecraftState state) {
203
204 final ParameterFunctionParameterFunction">ParameterFunction rangeError = new ParameterFunction() {
205
206 @Override
207 public double value(final ParameterDriver parameterDriver) {
208 return rangeErrorTroposphericModel(station, state);
209 }
210 };
211
212 final ParameterFunction rangeErrorDerivative =
213 Differentiation.differentiate(rangeError, 3, 10.0 * driver.getScale());
214
215 return rangeErrorDerivative.value(driver);
216
217 }
218
219
220
221
222
223
224
225
226 private double[] rangeErrorParameterDerivative(final double[] derivatives, final int freeStateParameters) {
227
228
229
230 final int dim = derivatives.length - 1 - freeStateParameters;
231 final double[] rangeError = new double[dim];
232
233 for (int i = 0; i < dim; i++) {
234 rangeError[i] = derivatives[1 + freeStateParameters + i];
235 }
236
237 return rangeError;
238 }
239
240
241 @Override
242 public List<ParameterDriver> getParametersDrivers() {
243 return tropoModel.getParametersDrivers();
244 }
245
246
247 @Override
248 public void modify(final EstimatedMeasurement<Range> estimated) {
249 final Range measurement = estimated.getObservedMeasurement();
250 final GroundStation station = measurement.getStation();
251 final SpacecraftState state = estimated.getStates()[0];
252
253 final double[] oldValue = estimated.getEstimatedValue();
254
255
256 final TroposphericDSConverterTroposphericDSConverter.html#TroposphericDSConverter">TroposphericDSConverter converter = new TroposphericDSConverter(state, 6, Propagator.DEFAULT_LAW);
257 final FieldSpacecraftState<DerivativeStructure> dsState = converter.getState(tropoModel);
258 final DerivativeStructure[] dsParameters = converter.getParameters(dsState, tropoModel);
259 final DerivativeStructure dsDelay = rangeErrorTroposphericModel(station, dsState, dsParameters);
260 final double[] derivatives = dsDelay.getAllDerivatives();
261
262 double[][] djac = new double[1][6];
263
264
265 if (tropoModel instanceof TroposphericModel) {
266 djac = rangeErrorJacobianState(station, state);
267 } else {
268 djac = rangeErrorJacobianState(derivatives, converter.getFreeStateParameters());
269 }
270
271 final double[][] stateDerivatives = estimated.getStateDerivatives(0);
272 for (int irow = 0; irow < stateDerivatives.length; ++irow) {
273 for (int jcol = 0; jcol < stateDerivatives[0].length; ++jcol) {
274 stateDerivatives[irow][jcol] += djac[irow][jcol];
275 }
276 }
277 estimated.setStateDerivatives(0, stateDerivatives);
278
279 int index = 0;
280 for (final ParameterDriver driver : getParametersDrivers()) {
281 if (driver.isSelected()) {
282
283 double parameterDerivative = estimated.getParameterDerivatives(driver)[0];
284 final double[] dDelaydP = rangeErrorParameterDerivative(derivatives, converter.getFreeStateParameters());
285 parameterDerivative += dDelaydP[index];
286 estimated.setParameterDerivatives(driver, parameterDerivative);
287 index = index + 1;
288 }
289
290 }
291
292 for (final ParameterDriver driver : Arrays.asList(station.getClockOffsetDriver(),
293 station.getEastOffsetDriver(),
294 station.getNorthOffsetDriver(),
295 station.getZenithOffsetDriver())) {
296 if (driver.isSelected()) {
297
298 double parameterDerivative = estimated.getParameterDerivatives(driver)[0];
299 parameterDerivative += rangeErrorParameterDerivative(station, driver, state);
300 estimated.setParameterDerivatives(driver, parameterDerivative);
301 }
302 }
303
304
305
306 final double[] newValue = oldValue.clone();
307 newValue[0] = newValue[0] + dsDelay.getReal();
308 estimated.setEstimatedValue(newValue);
309
310 }
311
312 }