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.exception.MathRuntimeException;
20 import org.hipparchus.filtering.kalman.ProcessEstimate;
21 import org.hipparchus.filtering.kalman.extended.ExtendedKalmanFilter;
22 import org.hipparchus.filtering.kalman.extended.NonLinearEvolution;
23 import org.hipparchus.filtering.kalman.extended.NonLinearProcess;
24 import org.hipparchus.linear.Array2DRowRealMatrix;
25 import org.hipparchus.linear.ArrayRealVector;
26 import org.hipparchus.linear.MatrixUtils;
27 import org.hipparchus.linear.QRDecomposition;
28 import org.hipparchus.linear.RealMatrix;
29 import org.hipparchus.linear.RealVector;
30 import org.hipparchus.util.FastMath;
31 import org.orekit.errors.OrekitException;
32 import org.orekit.estimation.measurements.EstimatedMeasurement;
33 import org.orekit.estimation.measurements.ObservedMeasurement;
34 import org.orekit.orbits.Orbit;
35 import org.orekit.orbits.OrbitType;
36 import org.orekit.propagation.PropagationType;
37 import org.orekit.propagation.SpacecraftState;
38 import org.orekit.propagation.conversion.DSSTPropagatorBuilder;
39 import org.orekit.propagation.semianalytical.dsst.DSSTHarvester;
40 import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
41 import org.orekit.propagation.semianalytical.dsst.forces.DSSTForceModel;
42 import org.orekit.propagation.semianalytical.dsst.forces.ShortPeriodTerms;
43 import org.orekit.propagation.semianalytical.dsst.utilities.AuxiliaryElements;
44 import org.orekit.time.AbsoluteDate;
45 import org.orekit.time.ChronologicalComparator;
46 import org.orekit.utils.ParameterDriver;
47 import org.orekit.utils.ParameterDriversList;
48 import org.orekit.utils.ParameterDriversList.DelegatingDriver;
49 import org.orekit.utils.TimeSpanMap.Span;
50
51 import java.util.ArrayList;
52 import java.util.Comparator;
53 import java.util.HashMap;
54 import java.util.List;
55 import java.util.Map;
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71 public class SemiAnalyticalKalmanModel implements KalmanEstimation, NonLinearProcess<MeasurementDecorator>, SemiAnalyticalProcess {
72
73
74 private final DSSTPropagatorBuilder builder;
75
76
77 private final ParameterDriversList estimatedOrbitalParameters;
78
79
80 private final ParameterDriversList estimatedPropagationParameters;
81
82
83 private final ParameterDriversList estimatedMeasurementsParameters;
84
85
86 private final Map<String, Integer> propagationParameterColumns;
87
88
89 private final Map<String, Integer> measurementParameterColumns;
90
91
92 private final double[] scale;
93
94
95 private final CovarianceMatrixProvider covarianceMatrixProvider;
96
97
98 private final CovarianceMatrixProvider measurementProcessNoiseMatrix;
99
100
101 private DSSTHarvester harvester;
102
103
104 private DSSTPropagator dsstPropagator;
105
106
107 private KalmanObserver observer;
108
109
110 private int currentMeasurementNumber;
111
112
113 private AbsoluteDate currentDate;
114
115
116 private RealVector predictedFilterCorrection;
117
118
119 private RealVector correctedFilterCorrection;
120
121
122 private EstimatedMeasurement<?> predictedMeasurement;
123
124
125 private EstimatedMeasurement<?> correctedMeasurement;
126
127
128 private SpacecraftState nominalMeanSpacecraftState;
129
130
131 private SpacecraftState previousNominalMeanSpacecraftState;
132
133
134 private ProcessEstimate correctedEstimate;
135
136
137 private RealMatrix phiS;
138
139
140 private RealMatrix psiS;
141
142
143
144
145
146
147
148 protected SemiAnalyticalKalmanModel(final DSSTPropagatorBuilder propagatorBuilder,
149 final CovarianceMatrixProvider covarianceMatrixProvider,
150 final ParameterDriversList estimatedMeasurementParameters,
151 final CovarianceMatrixProvider measurementProcessNoiseMatrix) {
152
153 this.builder = propagatorBuilder;
154 this.estimatedMeasurementsParameters = estimatedMeasurementParameters;
155 this.measurementParameterColumns = new HashMap<>(estimatedMeasurementsParameters.getDrivers().size());
156 this.observer = null;
157 this.currentMeasurementNumber = 0;
158 this.currentDate = propagatorBuilder.getInitialOrbitDate();
159 this.covarianceMatrixProvider = covarianceMatrixProvider;
160 this.measurementProcessNoiseMatrix = measurementProcessNoiseMatrix;
161
162
163 int columns = 0;
164
165
166 estimatedOrbitalParameters = new ParameterDriversList();
167 for (final ParameterDriver driver : builder.getOrbitalParametersDrivers().getDrivers()) {
168
169
170 if (driver.getReferenceDate() == null) {
171 driver.setReferenceDate(currentDate);
172 }
173
174
175 if (driver.isSelected()) {
176 estimatedOrbitalParameters.add(driver);
177 columns++;
178 }
179
180 }
181
182
183 estimatedPropagationParameters = new ParameterDriversList();
184 final List<String> estimatedPropagationParametersNames = new ArrayList<>();
185 for (final ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {
186
187
188 if (driver.getReferenceDate() == null) {
189 driver.setReferenceDate(currentDate);
190 }
191
192
193 if (driver.isSelected()) {
194 estimatedPropagationParameters.add(driver);
195
196 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
197
198 if (!estimatedPropagationParametersNames.contains(span.getData())) {
199 estimatedPropagationParametersNames.add(span.getData());
200 }
201 }
202 }
203
204 }
205 estimatedPropagationParametersNames.sort(Comparator.naturalOrder());
206
207
208 propagationParameterColumns = new HashMap<>(estimatedPropagationParametersNames.size());
209 for (final String driverName : estimatedPropagationParametersNames) {
210 propagationParameterColumns.put(driverName, columns);
211 ++columns;
212 }
213
214
215 for (final ParameterDriver parameter : estimatedMeasurementsParameters.getDrivers()) {
216 if (parameter.getReferenceDate() == null) {
217 parameter.setReferenceDate(currentDate);
218 }
219 for (Span<String> span = parameter.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
220 measurementParameterColumns.put(span.getData(), columns);
221 ++columns;
222 }
223 }
224
225
226 this.scale = new double[columns];
227 int index = 0;
228 for (final ParameterDriver driver : estimatedOrbitalParameters.getDrivers()) {
229 scale[index++] = driver.getScale();
230 }
231 for (final ParameterDriver driver : estimatedPropagationParameters.getDrivers()) {
232 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
233 scale[index++] = driver.getScale();
234 }
235 }
236 for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
237 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
238 scale[index++] = driver.getScale();
239 }
240 }
241
242
243 updateReferenceTrajectory(getEstimatedPropagator());
244 this.nominalMeanSpacecraftState = dsstPropagator.getInitialState();
245 this.previousNominalMeanSpacecraftState = nominalMeanSpacecraftState;
246
247
248 harvester.initializeFieldShortPeriodTerms(nominalMeanSpacecraftState);
249
250
251 this.predictedFilterCorrection = MatrixUtils.createRealVector(columns);
252 this.correctedFilterCorrection = predictedFilterCorrection;
253
254
255 this.psiS = null;
256 if (estimatedPropagationParameters.getNbParams() != 0) {
257 this.psiS = MatrixUtils.createRealMatrix(getNumberSelectedOrbitalDriversValuesToEstimate(),
258 getNumberSelectedPropagationDriversValuesToEstimate());
259 }
260
261
262 this.phiS = MatrixUtils.createRealIdentityMatrix(getNumberSelectedOrbitalDriversValuesToEstimate());
263
264
265 final int nbMeas = getNumberSelectedMeasurementDriversValuesToEstimate();
266
267
268 final int nbDyn = getNumberSelectedOrbitalDriversValuesToEstimate() + getNumberSelectedPropagationDriversValuesToEstimate();
269
270
271 final RealMatrix noiseK = MatrixUtils.createRealMatrix(nbDyn + nbMeas, nbDyn + nbMeas);
272 final RealMatrix noiseP = covarianceMatrixProvider.getInitialCovarianceMatrix(nominalMeanSpacecraftState);
273 noiseK.setSubMatrix(noiseP.getData(), 0, 0);
274 if (measurementProcessNoiseMatrix != null) {
275 final RealMatrix noiseM = measurementProcessNoiseMatrix.getInitialCovarianceMatrix(nominalMeanSpacecraftState);
276 noiseK.setSubMatrix(noiseM.getData(), nbDyn, nbDyn);
277 }
278
279
280 KalmanEstimatorUtil.checkDimension(noiseK.getRowDimension(),
281 builder.getOrbitalParametersDrivers(),
282 builder.getPropagationParametersDrivers(),
283 estimatedMeasurementsParameters);
284
285 final RealMatrix correctedCovariance = KalmanEstimatorUtil.normalizeCovarianceMatrix(noiseK, scale);
286
287
288 this.correctedEstimate = new ProcessEstimate(0.0, correctedFilterCorrection, correctedCovariance);
289
290 }
291
292
293 @Override
294 public KalmanObserver getObserver() {
295 return observer;
296 }
297
298
299
300
301 public void setObserver(final KalmanObserver observer) {
302 this.observer = observer;
303 }
304
305
306
307
308 public ProcessEstimate getEstimate() {
309 return correctedEstimate;
310 }
311
312
313
314
315 protected double[] getScale() {
316 return scale;
317 }
318
319
320
321
322
323
324
325
326
327 public DSSTPropagator processMeasurements(final List<ObservedMeasurement<?>> observedMeasurements,
328 final ExtendedKalmanFilter<MeasurementDecorator> filter) {
329 try {
330
331
332 observedMeasurements.sort(new ChronologicalComparator());
333 final AbsoluteDate tStart = observedMeasurements.get(0).getDate();
334 final AbsoluteDate tEnd = observedMeasurements.get(observedMeasurements.size() - 1).getDate();
335 final double overshootTimeRange = FastMath.nextAfter(tEnd.durationFrom(tStart),
336 Double.POSITIVE_INFINITY);
337
338
339 final SemiAnalyticalMeasurementHandler stepHandler = new SemiAnalyticalMeasurementHandler(this, filter, observedMeasurements, builder.getInitialOrbitDate());
340 dsstPropagator.getMultiplexer().add(stepHandler);
341 dsstPropagator.propagate(tStart, tStart.shiftedBy(overshootTimeRange));
342
343
344 return getEstimatedPropagator();
345
346 } catch (MathRuntimeException mrte) {
347 throw new OrekitException(mrte);
348 }
349 }
350
351
352
353
354 public DSSTPropagator getEstimatedPropagator() {
355
356 return (DSSTPropagator) builder.buildPropagator();
357 }
358
359
360 @Override
361 public NonLinearEvolution getEvolution(final double previousTime, final RealVector previousState,
362 final MeasurementDecorator measurement) {
363
364
365 final ObservedMeasurement<?> observedMeasurement = measurement.getObservedMeasurement();
366 for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
367 if (driver.getReferenceDate() == null) {
368 driver.setReferenceDate(builder.getInitialOrbitDate());
369 }
370 }
371
372
373 ++currentMeasurementNumber;
374
375
376 currentDate = measurement.getObservedMeasurement().getDate();
377
378
379 final RealMatrix stm = getErrorStateTransitionMatrix();
380
381
382 predictedFilterCorrection = predictFilterCorrection(stm);
383
384
385 analyticalDerivativeComputations(nominalMeanSpacecraftState);
386
387
388 final double[] osculating = computeOsculatingElements(predictedFilterCorrection);
389 final Orbit osculatingOrbit = OrbitType.EQUINOCTIAL.mapArrayToOrbit(osculating, null, builder.getPositionAngleType(),
390 currentDate, nominalMeanSpacecraftState.getOrbit().getMu(),
391 nominalMeanSpacecraftState.getFrame());
392
393
394 predictedMeasurement = observedMeasurement.estimate(currentMeasurementNumber,
395 currentMeasurementNumber,
396 new SpacecraftState[] {
397 new SpacecraftState(osculatingOrbit,
398 nominalMeanSpacecraftState.getAttitude(),
399 nominalMeanSpacecraftState.getMass(),
400 nominalMeanSpacecraftState.getAdditionalDataValues(),
401 nominalMeanSpacecraftState.getAdditionalStatesDerivatives())
402 });
403
404
405 final RealMatrix measurementMatrix = getMeasurementMatrix();
406
407
408 final int nbMeas = getNumberSelectedMeasurementDriversValuesToEstimate();
409
410
411 final int nbDyn = getNumberSelectedOrbitalDriversValuesToEstimate() + getNumberSelectedPropagationDriversValuesToEstimate();
412
413
414 final RealMatrix noiseK = MatrixUtils.createRealMatrix(nbDyn + nbMeas, nbDyn + nbMeas);
415 final RealMatrix noiseP = covarianceMatrixProvider.getProcessNoiseMatrix(previousNominalMeanSpacecraftState, nominalMeanSpacecraftState);
416 noiseK.setSubMatrix(noiseP.getData(), 0, 0);
417 if (measurementProcessNoiseMatrix != null) {
418 final RealMatrix noiseM = measurementProcessNoiseMatrix.getProcessNoiseMatrix(previousNominalMeanSpacecraftState, nominalMeanSpacecraftState);
419 noiseK.setSubMatrix(noiseM.getData(), nbDyn, nbDyn);
420 }
421
422
423 KalmanEstimatorUtil.checkDimension(noiseK.getRowDimension(),
424 builder.getOrbitalParametersDrivers(),
425 builder.getPropagationParametersDrivers(),
426 estimatedMeasurementsParameters);
427
428 final RealMatrix normalizedProcessNoise = KalmanEstimatorUtil.normalizeCovarianceMatrix(noiseK, scale);
429
430
431 return new NonLinearEvolution(measurement.getTime(), predictedFilterCorrection, stm,
432 normalizedProcessNoise, measurementMatrix);
433 }
434
435
436 @Override
437 public RealVector getInnovation(final MeasurementDecorator measurement, final NonLinearEvolution evolution,
438 final RealMatrix innovationCovarianceMatrix) {
439
440
441 KalmanEstimatorUtil.applyDynamicOutlierFilter(predictedMeasurement, innovationCovarianceMatrix);
442
443 return KalmanEstimatorUtil.computeInnovationVector(predictedMeasurement, predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation());
444 }
445
446
447 @Override
448 public void finalizeEstimation(final ObservedMeasurement<?> observedMeasurement,
449 final ProcessEstimate estimate) {
450
451 correctedEstimate = estimate;
452
453 correctedFilterCorrection = estimate.getState();
454
455 previousNominalMeanSpacecraftState = nominalMeanSpacecraftState;
456
457 final double[] osculating = computeOsculatingElements(correctedFilterCorrection);
458 final Orbit osculatingOrbit = OrbitType.EQUINOCTIAL.mapArrayToOrbit(osculating, null, builder.getPositionAngleType(),
459 currentDate, nominalMeanSpacecraftState.getOrbit().getMu(),
460 nominalMeanSpacecraftState.getFrame());
461
462
463 correctedMeasurement = observedMeasurement.estimate(currentMeasurementNumber,
464 currentMeasurementNumber,
465 new SpacecraftState[] {
466 new SpacecraftState(osculatingOrbit,
467 nominalMeanSpacecraftState.getAttitude(),
468 nominalMeanSpacecraftState.getMass(),
469 nominalMeanSpacecraftState.getAdditionalDataValues(),
470 nominalMeanSpacecraftState.getAdditionalStatesDerivatives())
471 });
472
473 if (observer != null) {
474 observer.evaluationPerformed(this);
475 }
476 }
477
478
479 @Override
480 public void finalizeOperationsObservationGrid() {
481
482 updateParameters();
483 }
484
485
486 @Override
487 public ParameterDriversList getEstimatedOrbitalParameters() {
488 return estimatedOrbitalParameters;
489 }
490
491
492 @Override
493 public ParameterDriversList getEstimatedPropagationParameters() {
494 return estimatedPropagationParameters;
495 }
496
497
498 @Override
499 public ParameterDriversList getEstimatedMeasurementsParameters() {
500 return estimatedMeasurementsParameters;
501 }
502
503
504 @Override
505 public SpacecraftState[] getPredictedSpacecraftStates() {
506 return new SpacecraftState[] {nominalMeanSpacecraftState};
507 }
508
509
510 @Override
511 public SpacecraftState[] getCorrectedSpacecraftStates() {
512 return new SpacecraftState[] {getEstimatedPropagator().getInitialState()};
513 }
514
515
516 @Override
517 public RealVector getPhysicalEstimatedState() {
518
519
520
521 final RealVector physicalEstimatedState = new ArrayRealVector(scale.length);
522 int i = 0;
523 for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
524 for (Span<Double> span = driver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
525 physicalEstimatedState.setEntry(i++, span.getData());
526 }
527 }
528 for (final DelegatingDriver driver : getEstimatedPropagationParameters().getDrivers()) {
529 for (Span<Double> span = driver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
530 physicalEstimatedState.setEntry(i++, span.getData());
531 }
532 }
533 for (final DelegatingDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
534 for (Span<Double> span = driver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
535 physicalEstimatedState.setEntry(i++, span.getData());
536 }
537 }
538
539 return physicalEstimatedState;
540 }
541
542
543 @Override
544 public RealMatrix getPhysicalEstimatedCovarianceMatrix() {
545
546
547
548
549
550 return KalmanEstimatorUtil.unnormalizeCovarianceMatrix(correctedEstimate.getCovariance(), scale);
551 }
552
553
554 @Override
555 public RealMatrix getPhysicalStateTransitionMatrix() {
556
557
558
559
560 return correctedEstimate.getStateTransitionMatrix() == null ?
561 null : KalmanEstimatorUtil.unnormalizeStateTransitionMatrix(correctedEstimate.getStateTransitionMatrix(), scale);
562 }
563
564
565 @Override
566 public RealMatrix getPhysicalMeasurementJacobian() {
567
568
569
570
571
572
573 return correctedEstimate.getMeasurementJacobian() == null ?
574 null : KalmanEstimatorUtil.unnormalizeMeasurementJacobian(correctedEstimate.getMeasurementJacobian(),
575 scale,
576 correctedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation());
577 }
578
579
580 @Override
581 public RealMatrix getPhysicalInnovationCovarianceMatrix() {
582
583
584
585
586 return correctedEstimate.getInnovationCovariance() == null ?
587 null : KalmanEstimatorUtil.unnormalizeInnovationCovarianceMatrix(correctedEstimate.getInnovationCovariance(),
588 predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation());
589 }
590
591
592 @Override
593 public RealMatrix getPhysicalKalmanGain() {
594
595
596
597
598
599
600 return correctedEstimate.getKalmanGain() == null ?
601 null : KalmanEstimatorUtil.unnormalizeKalmanGainMatrix(correctedEstimate.getKalmanGain(),
602 scale,
603 correctedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation());
604 }
605
606
607 @Override
608 public int getCurrentMeasurementNumber() {
609 return currentMeasurementNumber;
610 }
611
612
613 @Override
614 public AbsoluteDate getCurrentDate() {
615 return currentDate;
616 }
617
618
619 @Override
620 public EstimatedMeasurement<?> getPredictedMeasurement() {
621 return predictedMeasurement;
622 }
623
624
625 @Override
626 public EstimatedMeasurement<?> getCorrectedMeasurement() {
627 return correctedMeasurement;
628 }
629
630
631 @Override
632 public void updateNominalSpacecraftState(final SpacecraftState nominal) {
633 this.nominalMeanSpacecraftState = nominal;
634
635 builder.resetOrbit(nominal.getOrbit(), PropagationType.MEAN);
636
637
638
639
640 builder.setMass(nominal.getMass());
641 }
642
643
644
645
646 public void updateReferenceTrajectory(final DSSTPropagator propagator) {
647
648 dsstPropagator = propagator;
649
650
651 final String equationName = SemiAnalyticalKalmanEstimator.class.getName() + "-derivatives-";
652
653
654 final SpacecraftState meanState = dsstPropagator.initialIsOsculating() ?
655 DSSTPropagator.computeMeanState(dsstPropagator.getInitialState(), dsstPropagator.getAttitudeProvider(), dsstPropagator.getAllForceModels()) :
656 dsstPropagator.getInitialState();
657
658
659 dsstPropagator.setInitialState(meanState, PropagationType.MEAN);
660 harvester = dsstPropagator.setupMatricesComputation(equationName, null, null);
661
662 }
663
664
665 @Override
666 public void updateShortPeriods(final SpacecraftState state) {
667
668 for (final DSSTForceModel model : builder.getAllForceModels()) {
669 model.updateShortPeriodTerms(model.getParametersAllValues(), state);
670 }
671 harvester.updateFieldShortPeriodTerms(state);
672 }
673
674
675 @Override
676 public void initializeShortPeriodicTerms(final SpacecraftState meanState) {
677 final List<ShortPeriodTerms> shortPeriodTerms = new ArrayList<>();
678
679 final PropagationType type = PropagationType.OSCULATING;
680 for (final DSSTForceModel force : builder.getAllForceModels()) {
681 shortPeriodTerms.addAll(force.initializeShortPeriodTerms(new AuxiliaryElements(meanState.getOrbit(), 1), type, force.getParameters(meanState.getDate())));
682 }
683 dsstPropagator.setShortPeriodTerms(shortPeriodTerms);
684
685 harvester.initializeFieldShortPeriodTerms(meanState, type);
686 }
687
688
689
690
691
692
693
694 private RealMatrix getErrorStateTransitionMatrix() {
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714 final RealMatrix stm = MatrixUtils.createRealIdentityMatrix(correctedEstimate.getState().getDimension());
715
716
717 final int nbOrb = getNumberSelectedOrbitalDriversValuesToEstimate();
718 final RealMatrix dYdY0 = harvester.getB2(nominalMeanSpacecraftState);
719
720
721 final RealMatrix phi = dYdY0.multiply(phiS);
722
723
724 final List<DelegatingDriver> drivers = builder.getOrbitalParametersDrivers().getDrivers();
725 for (int i = 0; i < nbOrb; ++i) {
726 if (drivers.get(i).isSelected()) {
727 int jOrb = 0;
728 for (int j = 0; j < nbOrb; ++j) {
729 if (drivers.get(j).isSelected()) {
730 stm.setEntry(i, jOrb++, phi.getEntry(i, j));
731 }
732 }
733 }
734 }
735
736
737 phiS = new QRDecomposition(dYdY0).getSolver().getInverse();
738
739
740 if (psiS != null) {
741
742 final int nbProp = getNumberSelectedPropagationDriversValuesToEstimate();
743 final RealMatrix dYdPp = harvester.getB3(nominalMeanSpacecraftState);
744
745
746 final RealMatrix psi = dYdPp.subtract(phi.multiply(psiS));
747
748
749 for (int i = 0; i < nbOrb; ++i) {
750 for (int j = 0; j < nbProp; ++j) {
751 stm.setEntry(i, j + nbOrb, psi.getEntry(i, j));
752 }
753 }
754
755
756 psiS = dYdPp;
757
758 }
759
760
761
762 for (int i = 0; i < scale.length; i++) {
763 for (int j = 0; j < scale.length; j++ ) {
764 stm.setEntry(i, j, stm.getEntry(i, j) * scale[j] / scale[i]);
765 }
766 }
767
768
769 return stm;
770
771 }
772
773
774
775
776
777
778 private RealMatrix getMeasurementMatrix() {
779
780
781 final SpacecraftState evaluationState = predictedMeasurement.getStates()[0];
782 final ObservedMeasurement<?> observedMeasurement = predictedMeasurement.getObservedMeasurement();
783 final double[] sigma = predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation();
784
785
786
787
788 final RealMatrix measurementMatrix = MatrixUtils.
789 createRealMatrix(observedMeasurement.getDimension(),
790 correctedEstimate.getState().getDimension());
791
792
793 final Orbit predictedOrbit = evaluationState.getOrbit();
794
795
796
797
798
799 final int nbOrb = getNumberSelectedOrbitalDrivers();
800 final int nbProp = getNumberSelectedPropagationDrivers();
801 final double[][] aCY = new double[nbOrb][nbOrb];
802 predictedOrbit.getJacobianWrtParameters(builder.getPositionAngleType(), aCY);
803 final RealMatrix dCdY = new Array2DRowRealMatrix(aCY, false);
804
805
806 final RealMatrix dMdC = new Array2DRowRealMatrix(predictedMeasurement.getStateDerivatives(0), false);
807
808
809 RealMatrix dMdY = dMdC.multiply(dCdY);
810
811
812 final RealMatrix IpB1B4 = MatrixUtils.createRealMatrix(nbOrb, nbOrb + nbProp);
813
814
815 final RealMatrix B1 = harvester.getB1();
816
817
818 final RealMatrix I = MatrixUtils.createRealIdentityMatrix(nbOrb);
819 final RealMatrix IpB1 = I.add(B1);
820 IpB1B4.setSubMatrix(IpB1.getData(), 0, 0);
821
822
823 if (psiS != null) {
824 final RealMatrix B4 = harvester.getB4();
825 IpB1B4.setSubMatrix(B4.getData(), 0, nbOrb);
826 }
827
828
829 dMdY = dMdY.multiply(IpB1B4);
830
831 for (int i = 0; i < dMdY.getRowDimension(); i++) {
832 for (int j = 0; j < nbOrb; j++) {
833 final double driverScale = builder.getOrbitalParametersDrivers().getDrivers().get(j).getScale();
834 measurementMatrix.setEntry(i, j, dMdY.getEntry(i, j) / sigma[i] * driverScale);
835 }
836
837 int col = 0;
838 for (int j = 0; j < nbProp; j++) {
839 final double driverScale = estimatedPropagationParameters.getDrivers().get(j).getScale();
840 for (Span<Double> span = estimatedPropagationParameters.getDrivers().get(j).getValueSpanMap().getFirstSpan();
841 span != null; span = span.next()) {
842
843 measurementMatrix.setEntry(i, col + nbOrb,
844 dMdY.getEntry(i, col + nbOrb) / sigma[i] * driverScale);
845 col++;
846 }
847 }
848 }
849
850
851
852
853
854
855 for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
856 if (driver.isSelected()) {
857 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
858
859 final double[] aMPm = predictedMeasurement.getParameterDerivatives(driver, span.getStart());
860
861
862 if (measurementParameterColumns.get(span.getData()) != null) {
863
864 final int driverColumn = measurementParameterColumns.get(span.getData());
865
866
867 for (int i = 0; i < aMPm.length; ++i) {
868 measurementMatrix.setEntry(i, driverColumn, aMPm[i] / sigma[i] * driver.getScale());
869 }
870 }
871 }
872 }
873 }
874
875 return measurementMatrix;
876 }
877
878
879
880
881
882 private RealVector predictFilterCorrection(final RealMatrix stm) {
883
884 return stm.operate(correctedFilterCorrection);
885 }
886
887
888
889
890
891 private double[] computeOsculatingElements(final RealVector filterCorrection) {
892
893
894 final int nbOrb = getNumberSelectedOrbitalDrivers();
895
896
897 final RealMatrix B1 = harvester.getB1();
898
899
900 final double[] shortPeriodTerms = dsstPropagator.getShortPeriodTermsValue(nominalMeanSpacecraftState);
901
902
903 final RealVector physicalFilterCorrection = MatrixUtils.createRealVector(nbOrb);
904 for (int index = 0; index < nbOrb; index++) {
905 physicalFilterCorrection.addToEntry(index, filterCorrection.getEntry(index) * scale[index]);
906 }
907
908
909 final RealVector B1Correction = B1.operate(physicalFilterCorrection);
910
911
912 final double[] nominalMeanElements = new double[nbOrb];
913 OrbitType.EQUINOCTIAL.mapOrbitToArray(nominalMeanSpacecraftState.getOrbit(), builder.getPositionAngleType(), nominalMeanElements, null);
914
915
916 final double[] osculatingElements = new double[nbOrb];
917 for (int i = 0; i < nbOrb; i++) {
918 osculatingElements[i] = nominalMeanElements[i] +
919 physicalFilterCorrection.getEntry(i) +
920 shortPeriodTerms[i] +
921 B1Correction.getEntry(i);
922 }
923
924
925 return osculatingElements;
926
927 }
928
929
930
931
932
933 private void analyticalDerivativeComputations(final SpacecraftState state) {
934 harvester.setReferenceState(state);
935 }
936
937
938
939
940 private int getNumberSelectedOrbitalDrivers() {
941 return estimatedOrbitalParameters.getNbParams();
942 }
943
944
945
946
947 private int getNumberSelectedPropagationDrivers() {
948 return estimatedPropagationParameters.getNbParams();
949 }
950
951
952
953
954
955
956 private int getNumberSelectedOrbitalDriversValuesToEstimate() {
957 int nbOrbitalValuesToEstimate = 0;
958 for (final ParameterDriver driver : estimatedOrbitalParameters.getDrivers()) {
959 nbOrbitalValuesToEstimate += driver.getNbOfValues();
960 }
961 return nbOrbitalValuesToEstimate;
962 }
963
964
965
966
967
968
969 private int getNumberSelectedPropagationDriversValuesToEstimate() {
970 int nbPropagationValuesToEstimate = 0;
971 for (final ParameterDriver driver : estimatedPropagationParameters.getDrivers()) {
972 nbPropagationValuesToEstimate += driver.getNbOfValues();
973 }
974 return nbPropagationValuesToEstimate;
975 }
976
977
978
979
980
981
982 private int getNumberSelectedMeasurementDriversValuesToEstimate() {
983 int nbMeasurementValuesToEstimate = 0;
984 for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
985 nbMeasurementValuesToEstimate += driver.getNbOfValues();
986 }
987 return nbMeasurementValuesToEstimate;
988 }
989
990
991
992
993 private void updateParameters() {
994 final RealVector correctedState = correctedEstimate.getState();
995 int i = 0;
996
997 for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
998
999 for (Span<Double> span = driver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
1000 driver.setNormalizedValue(driver.getNormalizedValue(span.getStart()) + correctedState.getEntry(i++), span.getStart());
1001 }
1002 }
1003
1004
1005 for (final DelegatingDriver driver : getEstimatedPropagationParameters().getDrivers()) {
1006
1007
1008 for (Span<Double> span = driver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
1009 driver.setNormalizedValue(driver.getNormalizedValue(span.getStart()) + correctedState.getEntry(i++), span.getStart());
1010 }
1011 }
1012
1013
1014 for (final DelegatingDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
1015
1016 for (Span<Double> span = driver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
1017 driver.setNormalizedValue(driver.getNormalizedValue(span.getStart()) + correctedState.getEntry(i++), span.getStart());
1018 }
1019 }
1020 }
1021
1022 }