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.linear.ArrayRealVector;
21 import org.hipparchus.linear.MatrixUtils;
22 import org.hipparchus.linear.RealMatrix;
23 import org.hipparchus.linear.RealVector;
24 import org.orekit.estimation.measurements.EstimatedMeasurement;
25 import org.orekit.propagation.Propagator;
26 import org.orekit.propagation.SpacecraftState;
27 import org.orekit.propagation.conversion.PropagatorBuilder;
28 import org.orekit.time.AbsoluteDate;
29 import org.orekit.utils.ParameterDriver;
30 import org.orekit.utils.ParameterDriversList;
31 import org.orekit.utils.ParameterDriversList.DelegatingDriver;
32
33 import java.util.ArrayList;
34 import java.util.Arrays;
35 import java.util.Comparator;
36 import java.util.HashMap;
37 import java.util.List;
38 import java.util.Map;
39
40
41
42
43
44
45 class KalmanEstimationCommon implements KalmanEstimation {
46
47
48 private final List<PropagatorBuilder> builders;
49
50
51 private final ParameterDriversList allEstimatedOrbitalParameters;
52
53
54 private final ParameterDriversList allEstimatedPropagationParameters;
55
56
57
58
59 private final ParameterDriversList[] estimatedOrbitalParameters;
60
61
62 private final ParameterDriversList[] estimatedPropagationParameters;
63
64
65 private final ParameterDriversList estimatedMeasurementsParameters;
66
67
68 private final int[] orbitsStartColumns;
69
70
71 private final int[] orbitsEndColumns;
72
73
74 private final Map<String, Integer> propagationParameterColumns;
75
76
77 private final Map<String, Integer> measurementParameterColumns;
78
79
80 private final List<CovarianceMatrixProvider> covarianceMatricesProviders;
81
82
83 private final CovarianceMatrixProvider measurementProcessNoiseMatrix;
84
85
86 private final int[][] covarianceIndirection;
87
88
89 private final double[] scale;
90
91
92 private ProcessEstimate correctedEstimate;
93
94
95 private int currentMeasurementNumber;
96
97
98 private final AbsoluteDate referenceDate;
99
100
101 private AbsoluteDate currentDate;
102
103
104 private final SpacecraftState[] predictedSpacecraftStates;
105
106
107 private final SpacecraftState[] correctedSpacecraftStates;
108
109
110 private EstimatedMeasurement<?> predictedMeasurement;
111
112
113 private EstimatedMeasurement<?> correctedMeasurement;
114
115
116
117
118
119
120
121 protected KalmanEstimationCommon(final List<PropagatorBuilder> propagatorBuilders,
122 final List<CovarianceMatrixProvider> covarianceMatricesProviders,
123 final ParameterDriversList estimatedMeasurementParameters,
124 final CovarianceMatrixProvider measurementProcessNoiseMatrix) {
125
126 this.builders = propagatorBuilders;
127 this.estimatedMeasurementsParameters = estimatedMeasurementParameters;
128 this.measurementParameterColumns = new HashMap<>(estimatedMeasurementsParameters.getDrivers().size());
129 this.currentMeasurementNumber = 0;
130 this.referenceDate = propagatorBuilders.get(0).getInitialOrbitDate();
131 this.currentDate = referenceDate;
132
133 final Map<String, Integer> orbitalParameterColumns = new HashMap<>(6 * builders.size());
134 orbitsStartColumns = new int[builders.size()];
135 orbitsEndColumns = new int[builders.size()];
136 int columns = 0;
137 allEstimatedOrbitalParameters = new ParameterDriversList();
138 estimatedOrbitalParameters = new ParameterDriversList[builders.size()];
139 for (int k = 0; k < builders.size(); ++k) {
140 estimatedOrbitalParameters[k] = new ParameterDriversList();
141 orbitsStartColumns[k] = columns;
142 final String suffix = propagatorBuilders.size() > 1 ? "[" + k + "]" : null;
143 for (final ParameterDriver driver : builders.get(k).getOrbitalParametersDrivers().getDrivers()) {
144 if (driver.getReferenceDate() == null) {
145 driver.setReferenceDate(currentDate);
146 }
147 if (suffix != null && !driver.getName().endsWith(suffix)) {
148
149
150 driver.setName(driver.getName() + suffix);
151 }
152 if (driver.isSelected()) {
153 allEstimatedOrbitalParameters.add(driver);
154 estimatedOrbitalParameters[k].add(driver);
155 orbitalParameterColumns.put(driver.getName(), columns++);
156 }
157 }
158 orbitsEndColumns[k] = columns;
159 }
160
161
162 allEstimatedPropagationParameters = new ParameterDriversList();
163 estimatedPropagationParameters = new ParameterDriversList[builders.size()];
164 final List<String> estimatedPropagationParametersNames = new ArrayList<>();
165 for (int k = 0; k < builders.size(); ++k) {
166 estimatedPropagationParameters[k] = new ParameterDriversList();
167 for (final ParameterDriver driver : builders.get(k).getPropagationParametersDrivers().getDrivers()) {
168 if (driver.getReferenceDate() == null) {
169 driver.setReferenceDate(currentDate);
170 }
171 if (driver.isSelected()) {
172 allEstimatedPropagationParameters.add(driver);
173 estimatedPropagationParameters[k].add(driver);
174 final String driverName = driver.getName();
175
176 if (!estimatedPropagationParametersNames.contains(driverName)) {
177 estimatedPropagationParametersNames.add(driverName);
178 }
179 }
180 }
181 }
182 estimatedPropagationParametersNames.sort(Comparator.naturalOrder());
183
184
185 propagationParameterColumns = new HashMap<>(estimatedPropagationParametersNames.size());
186 for (final String driverName : estimatedPropagationParametersNames) {
187 propagationParameterColumns.put(driverName, columns);
188 ++columns;
189 }
190
191
192 for (final ParameterDriver parameter : estimatedMeasurementsParameters.getDrivers()) {
193 if (parameter.getReferenceDate() == null) {
194 parameter.setReferenceDate(currentDate);
195 }
196 measurementParameterColumns.put(parameter.getName(), columns);
197 ++columns;
198 }
199
200
201 this.covarianceMatricesProviders = covarianceMatricesProviders;
202 this.measurementProcessNoiseMatrix = measurementProcessNoiseMatrix;
203 this.covarianceIndirection = new int[builders.size()][columns];
204 for (int k = 0; k < covarianceIndirection.length; ++k) {
205 final ParameterDriversList orbitDrivers = builders.get(k).getOrbitalParametersDrivers();
206 final ParameterDriversList parametersDrivers = builders.get(k).getPropagationParametersDrivers();
207 Arrays.fill(covarianceIndirection[k], -1);
208 int i = 0;
209 for (final ParameterDriver driver : orbitDrivers.getDrivers()) {
210 final Integer c = orbitalParameterColumns.get(driver.getName());
211 if (c != null) {
212 covarianceIndirection[k][i++] = c;
213 }
214 }
215 for (final ParameterDriver driver : parametersDrivers.getDrivers()) {
216 final Integer c = propagationParameterColumns.get(driver.getName());
217 if (c != null) {
218 covarianceIndirection[k][i++] = c;
219 }
220 }
221 for (final ParameterDriver driver : estimatedMeasurementParameters.getDrivers()) {
222 final Integer c = measurementParameterColumns.get(driver.getName());
223 if (c != null) {
224 covarianceIndirection[k][i++] = c;
225 }
226 }
227 }
228
229
230 this.scale = new double[columns];
231 int index = 0;
232 for (final ParameterDriver driver : allEstimatedOrbitalParameters.getDrivers()) {
233 scale[index++] = driver.getScale();
234 }
235 for (final ParameterDriver driver : allEstimatedPropagationParameters.getDrivers()) {
236 scale[index++] = driver.getScale();
237 }
238 for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
239 scale[index++] = driver.getScale();
240 }
241
242
243 this.predictedSpacecraftStates = new SpacecraftState[builders.size()];
244 for (int i = 0; i < builders.size(); ++i) {
245 predictedSpacecraftStates[i] = builders.get(i).buildPropagator().getInitialState();
246 }
247 this.correctedSpacecraftStates = predictedSpacecraftStates.clone();
248
249
250 final RealVector correctedState = MatrixUtils.createRealVector(columns);
251
252 int p = 0;
253 for (final ParameterDriver driver : allEstimatedOrbitalParameters.getDrivers()) {
254 correctedState.setEntry(p++, driver.getNormalizedValue());
255 }
256 for (final ParameterDriver driver : allEstimatedPropagationParameters.getDrivers()) {
257 correctedState.setEntry(p++, driver.getNormalizedValue());
258 }
259 for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
260 correctedState.setEntry(p++, driver.getNormalizedValue());
261 }
262
263
264 final RealMatrix physicalProcessNoise = MatrixUtils.createRealMatrix(columns, columns);
265 for (int k = 0; k < covarianceMatricesProviders.size(); ++k) {
266
267
268 final int nbMeas = estimatedMeasurementParameters.getNbParams();
269
270
271 final int nbDyn = orbitsEndColumns[k] - orbitsStartColumns[k] +
272 estimatedPropagationParameters[k].getNbParams();
273
274
275 final RealMatrix noiseK = MatrixUtils.createRealMatrix(nbDyn + nbMeas, nbDyn + nbMeas);
276 if (nbDyn > 0) {
277 final RealMatrix noiseP = covarianceMatricesProviders.get(k).
278 getInitialCovarianceMatrix(correctedSpacecraftStates[k]);
279 noiseK.setSubMatrix(noiseP.getData(), 0, 0);
280 }
281 if (measurementProcessNoiseMatrix != null) {
282 final RealMatrix noiseM = measurementProcessNoiseMatrix.
283 getInitialCovarianceMatrix(correctedSpacecraftStates[k]);
284 noiseK.setSubMatrix(noiseM.getData(), nbDyn, nbDyn);
285 }
286
287 KalmanEstimatorUtil.checkDimension(noiseK.getRowDimension(),
288 builders.get(k).getOrbitalParametersDrivers(),
289 builders.get(k).getPropagationParametersDrivers(),
290 estimatedMeasurementsParameters);
291
292 final int[] indK = covarianceIndirection[k];
293 for (int i = 0; i < indK.length; ++i) {
294 if (indK[i] >= 0) {
295 for (int j = 0; j < indK.length; ++j) {
296 if (indK[j] >= 0) {
297 physicalProcessNoise.setEntry(indK[i], indK[j], noiseK.getEntry(i, j));
298 }
299 }
300 }
301 }
302
303 }
304 final RealMatrix correctedCovariance = KalmanEstimatorUtil.normalizeCovarianceMatrix(physicalProcessNoise, scale);
305
306 correctedEstimate = new ProcessEstimate(0.0, correctedState, correctedCovariance);
307
308 }
309
310
311
312 @Override
313 public RealMatrix getPhysicalStateTransitionMatrix() {
314
315
316
317
318 return correctedEstimate.getStateTransitionMatrix() == null ?
319 null : KalmanEstimatorUtil.unnormalizeStateTransitionMatrix(correctedEstimate.getStateTransitionMatrix(), scale);
320 }
321
322
323 @Override
324 public RealMatrix getPhysicalMeasurementJacobian() {
325
326
327
328
329
330
331 return correctedEstimate.getMeasurementJacobian() == null ?
332 null : KalmanEstimatorUtil.unnormalizeMeasurementJacobian(correctedEstimate.getMeasurementJacobian(),
333 scale,
334 correctedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation());
335 }
336
337
338 @Override
339 public RealMatrix getPhysicalInnovationCovarianceMatrix() {
340
341
342
343
344 return correctedEstimate.getInnovationCovariance() == null ?
345 null : KalmanEstimatorUtil.unnormalizeInnovationCovarianceMatrix(correctedEstimate.getInnovationCovariance(),
346 predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation());
347 }
348
349
350 @Override
351 public RealMatrix getPhysicalKalmanGain() {
352
353
354
355
356
357
358 return correctedEstimate.getKalmanGain() == null ?
359 null : KalmanEstimatorUtil.unnormalizeKalmanGainMatrix(correctedEstimate.getKalmanGain(),
360 scale,
361 correctedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation());
362 }
363
364
365 @Override
366 public SpacecraftState[] getPredictedSpacecraftStates() {
367 return predictedSpacecraftStates.clone();
368 }
369
370
371 @Override
372 public SpacecraftState[] getCorrectedSpacecraftStates() {
373 return correctedSpacecraftStates.clone();
374 }
375
376
377 @Override
378 public int getCurrentMeasurementNumber() {
379 return currentMeasurementNumber;
380 }
381
382
383 @Override
384 public AbsoluteDate getCurrentDate() {
385 return currentDate;
386 }
387
388
389 @Override
390 public EstimatedMeasurement<?> getPredictedMeasurement() {
391 return predictedMeasurement;
392 }
393
394
395 @Override
396 public EstimatedMeasurement<?> getCorrectedMeasurement() {
397 return correctedMeasurement;
398 }
399
400
401 @Override
402 public RealVector getPhysicalEstimatedState() {
403
404
405
406 final RealVector physicalEstimatedState = new ArrayRealVector(scale.length);
407 int i = 0;
408 for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
409 physicalEstimatedState.setEntry(i++, driver.getValue());
410 }
411 for (final DelegatingDriver driver : getEstimatedPropagationParameters().getDrivers()) {
412 physicalEstimatedState.setEntry(i++, driver.getValue());
413 }
414 for (final DelegatingDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
415 physicalEstimatedState.setEntry(i++, driver.getValue());
416 }
417
418 return physicalEstimatedState;
419 }
420
421
422 @Override
423 public RealMatrix getPhysicalEstimatedCovarianceMatrix() {
424
425
426
427
428
429 return KalmanEstimatorUtil.unnormalizeCovarianceMatrix(correctedEstimate.getCovariance(), scale);
430 }
431
432
433 @Override
434 public ParameterDriversList getEstimatedOrbitalParameters() {
435 return allEstimatedOrbitalParameters;
436 }
437
438
439 @Override
440 public ParameterDriversList getEstimatedPropagationParameters() {
441 return allEstimatedPropagationParameters;
442 }
443
444
445 @Override
446 public ParameterDriversList getEstimatedMeasurementsParameters() {
447 return estimatedMeasurementsParameters;
448 }
449
450
451
452
453 public ProcessEstimate getEstimate() {
454 return correctedEstimate;
455 }
456
457
458
459
460 public List<PropagatorBuilder> getBuilders() {
461 return builders;
462 }
463
464
465
466
467 public Propagator[] getEstimatedPropagators() {
468
469 final Propagator[] propagators = new Propagator[getBuilders().size()];
470 for (int k = 0; k < getBuilders().size(); ++k) {
471 propagators[k] = getBuilders().get(k).buildPropagator();
472 }
473 return propagators;
474 }
475
476 protected RealMatrix getNormalizedProcessNoise(final int stateDimension) {
477 final RealMatrix physicalProcessNoise = MatrixUtils.createRealMatrix(stateDimension, stateDimension);
478 for (int k = 0; k < covarianceMatricesProviders.size(); ++k) {
479
480
481 final int nbMeas = estimatedMeasurementsParameters.getNbParams();
482
483
484 final int nbDyn = orbitsEndColumns[k] - orbitsStartColumns[k] +
485 estimatedPropagationParameters[k].getNbParams();
486
487
488 final RealMatrix noiseK = MatrixUtils.createRealMatrix(nbDyn + nbMeas, nbDyn + nbMeas);
489 if (nbDyn > 0) {
490 final RealMatrix noiseP = covarianceMatricesProviders.get(k).
491 getProcessNoiseMatrix(correctedSpacecraftStates[k],
492 predictedSpacecraftStates[k]);
493 noiseK.setSubMatrix(noiseP.getData(), 0, 0);
494 }
495 if (measurementProcessNoiseMatrix != null) {
496 final RealMatrix noiseM = measurementProcessNoiseMatrix.
497 getProcessNoiseMatrix(correctedSpacecraftStates[k],
498 predictedSpacecraftStates[k]);
499 noiseK.setSubMatrix(noiseM.getData(), nbDyn, nbDyn);
500 }
501
502 KalmanEstimatorUtil.checkDimension(noiseK.getRowDimension(),
503 builders.get(k).getOrbitalParametersDrivers(),
504 builders.get(k).getPropagationParametersDrivers(),
505 estimatedMeasurementsParameters);
506
507 final int[] indK = covarianceIndirection[k];
508 for (int i = 0; i < indK.length; ++i) {
509 if (indK[i] >= 0) {
510 for (int j = 0; j < indK.length; ++j) {
511 if (indK[j] >= 0) {
512 physicalProcessNoise.setEntry(indK[i], indK[j], noiseK.getEntry(i, j));
513 }
514 }
515 }
516 }
517
518 }
519 return KalmanEstimatorUtil.normalizeCovarianceMatrix(physicalProcessNoise, scale);
520 }
521
522
523 protected int[] getOrbitsStartColumns() {
524 return orbitsStartColumns;
525 }
526
527 protected Map<String, Integer> getPropagationParameterColumns() {
528 return propagationParameterColumns;
529 }
530
531 protected Map<String, Integer> getMeasurementParameterColumns() {
532 return measurementParameterColumns;
533 }
534
535 protected ParameterDriversList[] getEstimatedPropagationParametersArray() {
536 return estimatedPropagationParameters;
537 }
538
539 protected ParameterDriversList[] getEstimatedOrbitalParametersArray() {
540 return estimatedOrbitalParameters;
541 }
542
543 protected int[][] getCovarianceIndirection() {
544 return covarianceIndirection;
545 }
546
547 protected double[] getScale() {
548 return scale;
549 }
550
551 protected ProcessEstimate getCorrectedEstimate() {
552 return correctedEstimate;
553 }
554
555 protected void setCorrectedEstimate(final ProcessEstimate correctedEstimate) {
556 this.correctedEstimate = correctedEstimate;
557 }
558
559 protected AbsoluteDate getReferenceDate() {
560 return referenceDate;
561 }
562
563 protected void incrementCurrentMeasurementNumber() {
564 currentMeasurementNumber += 1;
565 }
566
567 public void setCurrentDate(final AbsoluteDate currentDate) {
568 this.currentDate = currentDate;
569 }
570
571 protected void setCorrectedSpacecraftState(final SpacecraftState correctedSpacecraftState, final int index) {
572 this.correctedSpacecraftStates[index] = correctedSpacecraftState;
573 }
574
575 protected void setPredictedSpacecraftState(final SpacecraftState predictedSpacecraftState, final int index) {
576 this.predictedSpacecraftStates[index] = predictedSpacecraftState;
577 }
578
579 protected void setPredictedMeasurement(final EstimatedMeasurement<?> predictedMeasurement) {
580 this.predictedMeasurement = predictedMeasurement;
581 }
582
583 protected void setCorrectedMeasurement(final EstimatedMeasurement<?> correctedMeasurement) {
584 this.correctedMeasurement = correctedMeasurement;
585 }
586 }