1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.sequential;
18
19 import org.hipparchus.filtering.kalman.ProcessEstimate;
20 import org.hipparchus.filtering.kalman.unscented.UnscentedEvolution;
21 import org.hipparchus.filtering.kalman.unscented.UnscentedProcess;
22 import org.hipparchus.linear.ArrayRealVector;
23 import org.hipparchus.linear.MatrixUtils;
24 import org.hipparchus.linear.RealMatrix;
25 import org.hipparchus.linear.RealVector;
26 import org.orekit.estimation.measurements.EstimatedMeasurement;
27 import org.orekit.estimation.measurements.EstimatedMeasurementBase;
28 import org.orekit.estimation.measurements.ObservedMeasurement;
29 import org.orekit.orbits.CartesianOrbit;
30 import org.orekit.orbits.Orbit;
31 import org.orekit.propagation.Propagator;
32 import org.orekit.propagation.SpacecraftState;
33 import org.orekit.propagation.conversion.AbstractPropagatorBuilder;
34 import org.orekit.propagation.conversion.PropagatorBuilder;
35 import org.orekit.time.AbsoluteDate;
36 import org.orekit.utils.ParameterDriver;
37 import org.orekit.utils.ParameterDriversList;
38 import org.orekit.utils.ParameterDriversList.DelegatingDriver;
39
40 import java.util.List;
41
42
43
44
45
46
47 public class UnscentedKalmanModel extends AbstractKalmanEstimationCommon implements UnscentedProcess<MeasurementDecorator> {
48
49
50 private final double[] referenceValues;
51
52
53
54
55
56
57
58 protected UnscentedKalmanModel(final List<PropagatorBuilder> propagatorBuilders,
59 final List<CovarianceMatrixProvider> covarianceMatricesProviders,
60 final ParameterDriversList estimatedMeasurementParameters,
61 final CovarianceMatrixProvider measurementProcessNoiseMatrix) {
62
63 super(propagatorBuilders, covarianceMatricesProviders, estimatedMeasurementParameters, measurementProcessNoiseMatrix);
64
65
66 int stateDimension = 0;
67 for (final ParameterDriver ignored : getEstimatedOrbitalParameters().getDrivers()) {
68 stateDimension += 1;
69 }
70 for (final ParameterDriver ignored : getEstimatedPropagationParameters().getDrivers()) {
71 stateDimension += 1;
72 }
73 for (final ParameterDriver ignored : getEstimatedMeasurementsParameters().getDrivers()) {
74 stateDimension += 1;
75 }
76
77 this.referenceValues = new double[stateDimension];
78 int index = 0;
79 for (final ParameterDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
80 referenceValues[index++] = driver.getReferenceValue();
81 }
82 for (final ParameterDriver driver : getEstimatedPropagationParameters().getDrivers()) {
83 referenceValues[index++] = driver.getReferenceValue();
84 }
85 for (final ParameterDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
86 referenceValues[index++] = driver.getReferenceValue();
87 }
88 }
89
90
91 @Override
92 public UnscentedEvolution getEvolution(final double previousTime, final RealVector[] sigmaPoints,
93 final MeasurementDecorator measurement) {
94
95
96 final ObservedMeasurement<?> observedMeasurement = measurement.getObservedMeasurement();
97 for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
98 if (driver.getReferenceDate() == null) {
99 driver.setReferenceDate(getBuilders().get(0).getInitialOrbitDate());
100 }
101 }
102
103
104 incrementCurrentMeasurementNumber();
105
106
107 setCurrentDate(measurement.getObservedMeasurement().getDate());
108
109
110 final RealVector[] predictedSigmaPoints = new RealVector[sigmaPoints.length];
111
112
113
114
115
116
117
118
119
120
121
122 for (int i = sigmaPoints.length - 1; i >= 0; i--) {
123
124
125 final RealVector sigmaPoint = sigmaPoints[i].copy();
126 updateParameters(sigmaPoint);
127
128
129 final Propagator[] propagators = getEstimatedPropagators();
130
131
132 predictedSigmaPoints[i] =
133 predictState(observedMeasurement.getDate(), sigmaPoint, propagators, i != 0);
134 }
135
136
137 int d = 0;
138 for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
139 driver.setReferenceValue(referenceValues[d]);
140 driver.setNormalizedValue(predictedSigmaPoints[0].getEntry(d));
141 referenceValues[d] = driver.getValue();
142
143
144 for (int i = 1; i < predictedSigmaPoints.length; ++i) {
145 predictedSigmaPoints[i].setEntry(d, predictedSigmaPoints[i].getEntry(d) - predictedSigmaPoints[0].getEntry(d));
146 }
147 predictedSigmaPoints[0].setEntry(d, 0.0);
148
149 d += 1;
150 }
151
152
153 return new UnscentedEvolution(measurement.getTime(), predictedSigmaPoints);
154 }
155
156
157 @Override
158 public RealMatrix getProcessNoiseMatrix(final double previousTime, final RealVector predictedState,
159 final MeasurementDecorator measurement) {
160
161 final RealVector predictedStateCopy = predictedState.copy();
162 updateParameters(predictedStateCopy);
163
164
165 Propagator[] propagators = getEstimatedPropagators();
166
167
168 for (int k = 0; k < propagators.length; ++k) {
169 final SpacecraftState predicted = propagators[k].getInitialState();
170 final Orbit predictedOrbit = getBuilders().get(k).getOrbitType().convertType(
171 new CartesianOrbit(predicted.getPVCoordinates(),
172 predicted.getFrame(),
173 measurement.getObservedMeasurement().getDate(),
174 predicted.getOrbit().getMu()
175 )
176 );
177 getBuilders().get(k).resetOrbit(predictedOrbit);
178 }
179 propagators = getEstimatedPropagators();
180
181
182 for (int k = 0; k < propagators.length; ++k) {
183 setPredictedSpacecraftState(propagators[k].getInitialState(), k);
184 }
185
186 return getNormalizedProcessNoise(predictedState.getDimension());
187 }
188
189
190 @Override
191 public RealVector[] getPredictedMeasurements(final RealVector[] predictedSigmaPoints, final MeasurementDecorator measurement) {
192
193
194 final ObservedMeasurement<?> observedMeasurement = measurement.getObservedMeasurement();
195
196
197 final RealVector theoreticalStandardDeviation =
198 MatrixUtils.createRealVector(observedMeasurement.getTheoreticalStandardDeviation());
199
200
201 final RealVector[] predictedMeasurements = new RealVector[predictedSigmaPoints.length];
202
203
204 for (int i = 0; i < predictedSigmaPoints.length; ++i) {
205
206 final RealVector predictedSigmaPoint = predictedSigmaPoints[i].copy();
207 updateParameters(predictedSigmaPoint);
208
209
210 final Propagator[] propagators = getEstimatedPropagators();
211
212
213 final SpacecraftState[] predictedStates = new SpacecraftState[propagators.length];
214 for (int k = 0; k < propagators.length; ++k) {
215 predictedStates[k] = propagators[k].getInitialState();
216 }
217
218
219 final EstimatedMeasurement<?> estimated = estimateMeasurement(observedMeasurement, getCurrentMeasurementNumber(),
220 KalmanEstimatorUtil.filterRelevant(observedMeasurement,
221 predictedStates));
222 predictedMeasurements[i] = new ArrayRealVector(estimated.getEstimatedValue())
223 .ebeDivide(theoreticalStandardDeviation);
224 }
225
226
227 return predictedMeasurements;
228
229 }
230
231
232 @Override
233 public RealVector getInnovation(final MeasurementDecorator measurement, final RealVector predictedMeas,
234 final RealVector predictedState, final RealMatrix innovationCovarianceMatrix) {
235
236 final RealVector theoreticalStandardDeviation =
237 MatrixUtils.createRealVector(measurement.getObservedMeasurement().getTheoreticalStandardDeviation());
238
239
240 final Propagator[] propagators = getEstimatedPropagators();
241
242
243 for (int k = 0; k < propagators.length; ++k) {
244 setPredictedSpacecraftState(propagators[k].getInitialState(), k);
245 }
246
247
248 final EstimatedMeasurement<?> predictedMeasurement =
249 estimateMeasurement(measurement.getObservedMeasurement(), getCurrentMeasurementNumber(),
250 KalmanEstimatorUtil.filterRelevant(measurement.getObservedMeasurement(),
251 getPredictedSpacecraftStates()));
252 setPredictedMeasurement(predictedMeasurement);
253 predictedMeasurement.setEstimatedValue(predictedMeas.ebeMultiply(theoreticalStandardDeviation).toArray());
254
255
256 KalmanEstimatorUtil.applyDynamicOutlierFilter(predictedMeasurement, innovationCovarianceMatrix);
257
258
259 return KalmanEstimatorUtil.computeInnovationVector(predictedMeasurement,
260 predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation());
261 }
262
263
264 private RealVector predictState(final AbsoluteDate date,
265 final RealVector previousState,
266 final Propagator[] propagators,
267 final boolean resetState) {
268
269
270 final RealVector predictedState = previousState.copy();
271
272
273 int jOrb = 0;
274
275
276 for (int k = 0; k < propagators.length; ++k) {
277
278
279 final SpacecraftState originalState = propagators[k].getInitialState();
280
281
282 final SpacecraftState predicted = propagators[k].propagate(date);
283
284
285
286 getBuilders().get(k).resetOrbit(predicted.getOrbit());
287
288
289
290
291 if (getBuilders().get(k) instanceof AbstractPropagatorBuilder) {
292 ((AbstractPropagatorBuilder<?>) (getBuilders().get(k))).setMass(predicted.getMass());
293 }
294
295
296
297
298
299 for (DelegatingDriver orbitalDriver : getBuilders().get(k).getOrbitalParametersDrivers().getDrivers()) {
300 if (orbitalDriver.isSelected()) {
301 orbitalDriver.setReferenceValue(referenceValues[jOrb]);
302 predictedState.setEntry(jOrb, orbitalDriver.getNormalizedValue());
303
304 jOrb += 1;
305 }
306 }
307
308
309 if (resetState) {
310 getBuilders().get(k).resetOrbit(originalState.getOrbit());
311 }
312 }
313
314 return predictedState;
315 }
316
317
318
319
320
321
322 public void finalizeEstimation(final ObservedMeasurement<?> observedMeasurement,
323 final ProcessEstimate estimate) {
324
325
326 setCorrectedEstimate(estimate);
327 updateParameters(estimate.getState());
328
329
330
331 final Propagator[] estimatedPropagators = getEstimatedPropagators();
332 for (int k = 0; k < estimatedPropagators.length; ++k) {
333 setCorrectedSpacecraftState(estimatedPropagators[k].getInitialState(), k);
334 }
335
336
337 setCorrectedMeasurement(estimateMeasurement(observedMeasurement, getCurrentMeasurementNumber(),
338 KalmanEstimatorUtil.filterRelevant(observedMeasurement,
339 getCorrectedSpacecraftStates())));
340 }
341
342
343
344
345
346
347
348
349
350
351 private static <T extends ObservedMeasurement<T>> EstimatedMeasurement<T> estimateMeasurement(final ObservedMeasurement<T> observedMeasurement,
352 final int measurementNumber,
353 final SpacecraftState[] spacecraftStates) {
354 final EstimatedMeasurementBase<T> estimatedMeasurementBase = observedMeasurement.
355 estimateWithoutDerivatives(measurementNumber, measurementNumber,
356 KalmanEstimatorUtil.filterRelevant(observedMeasurement, spacecraftStates));
357 return new EstimatedMeasurement<>(estimatedMeasurementBase);
358 }
359
360
361
362
363
364 private void updateParameters(final RealVector normalizedState) {
365 int i = 0;
366 for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
367
368 driver.setReferenceValue(referenceValues[i]);
369 driver.setNormalizedValue(normalizedState.getEntry(i));
370 normalizedState.setEntry(i++, driver.getNormalizedValue());
371 }
372 for (final DelegatingDriver driver : getEstimatedPropagationParameters().getDrivers()) {
373
374 driver.setNormalizedValue(normalizedState.getEntry(i));
375 normalizedState.setEntry(i++, driver.getNormalizedValue());
376 }
377 for (final DelegatingDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
378
379 driver.setNormalizedValue(normalizedState.getEntry(i));
380 normalizedState.setEntry(i++, driver.getNormalizedValue());
381 }
382 }
383 }