1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.common;
18
19 import java.util.Locale;
20 import java.util.Map;
21
22 import org.hipparchus.geometry.euclidean.threed.Vector3D;
23 import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem;
24 import org.orekit.estimation.leastsquares.BatchLSEstimator;
25 import org.orekit.estimation.leastsquares.BatchLSObserver;
26 import org.orekit.estimation.measurements.AngularAzEl;
27 import org.orekit.estimation.measurements.EstimatedMeasurement;
28 import org.orekit.estimation.measurements.EstimationsProvider;
29 import org.orekit.estimation.measurements.MultiplexedMeasurement;
30 import org.orekit.estimation.measurements.ObservedMeasurement;
31 import org.orekit.estimation.measurements.PV;
32 import org.orekit.estimation.measurements.Position;
33 import org.orekit.estimation.measurements.Range;
34 import org.orekit.estimation.measurements.RangeRate;
35 import org.orekit.orbits.Orbit;
36 import org.orekit.utils.PVCoordinates;
37 import org.orekit.utils.ParameterDriversList;
38
39
40
41
42 public class BatchLeastSquaresObserver implements BatchLSObserver {
43
44
45 private PVCoordinates previousPV;
46
47
48 private BatchLSEstimator estimator;
49
50
51
52
53
54
55
56 public BatchLeastSquaresObserver(final Orbit initialGuess, final BatchLSEstimator estimator,
57 final String header, final boolean print) {
58 this.previousPV = initialGuess.getPVCoordinates();
59 this.estimator = estimator;
60
61 if (print) {
62 System.out.format(Locale.US, header);
63 }
64 }
65
66
67 @Override
68 public void evaluationPerformed(final int iterationsCount, final int evaluationsCount,
69 final Orbit[] orbits,
70 final ParameterDriversList estimatedOrbitalParameters,
71 final ParameterDriversList estimatedPropagatorParameters,
72 final ParameterDriversList estimatedMeasurementsParameters,
73 final EstimationsProvider evaluationsProvider,
74 final LeastSquaresProblem.Evaluation lspEvaluation) {
75 final PVCoordinates currentPV = orbits[0].getPVCoordinates();
76 final String format0 = " %2d %2d %16.12f %s %s %s %s %s%n";
77 final String format = " %2d %2d %13.6f %12.9f %16.12f %s %s %s %s %s%n";
78 final EvaluationCounter<Range> rangeCounter = new EvaluationCounter<Range>();
79 final EvaluationCounter<RangeRate> rangeRateCounter = new EvaluationCounter<RangeRate>();
80 final EvaluationCounter<AngularAzEl> angularCounter = new EvaluationCounter<AngularAzEl>();
81 final EvaluationCounter<Position> positionCounter = new EvaluationCounter<Position>();
82 final EvaluationCounter<PV> pvCounter = new EvaluationCounter<PV>();
83 for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry : estimator.getLastEstimations().entrySet()) {
84 logEvaluation(entry.getValue(),
85 rangeCounter, rangeRateCounter, angularCounter, null, positionCounter, pvCounter, null);
86 }
87 if (evaluationsCount == 1) {
88 System.out.format(Locale.US, format0,
89 iterationsCount, evaluationsCount,
90 lspEvaluation.getRMS(),
91 rangeCounter.format(8), rangeRateCounter.format(8),
92 angularCounter.format(8), positionCounter.format(8),
93 pvCounter.format(8));
94 } else {
95 System.out.format(Locale.US, format,
96 iterationsCount, evaluationsCount,
97 Vector3D.distance(previousPV.getPosition(), currentPV.getPosition()),
98 Vector3D.distance(previousPV.getVelocity(), currentPV.getVelocity()),
99 lspEvaluation.getRMS(),
100 rangeCounter.format(8), rangeRateCounter.format(8),
101 angularCounter.format(8), positionCounter.format(8),
102 pvCounter.format(8));
103 }
104 previousPV = currentPV;
105 }
106
107
108
109 private void logEvaluation(EstimatedMeasurement<?> evaluation,
110 EvaluationLogger<Range> rangeLog,
111 EvaluationLogger<RangeRate> rangeRateLog,
112 EvaluationLogger<AngularAzEl> azimuthLog,
113 EvaluationLogger<AngularAzEl> elevationLog,
114 EvaluationLogger<Position> positionOnlyLog,
115 EvaluationLogger<PV> positionLog,
116 EvaluationLogger<PV> velocityLog) {
117 if (evaluation.getObservedMeasurement() instanceof Range) {
118 @SuppressWarnings("unchecked")
119 final EstimatedMeasurement<Range> ev = (EstimatedMeasurement<Range>) evaluation;
120 if (rangeLog != null) {
121 rangeLog.log(ev);
122 }
123 } else if (evaluation.getObservedMeasurement() instanceof RangeRate) {
124 @SuppressWarnings("unchecked")
125 final EstimatedMeasurement<RangeRate> ev = (EstimatedMeasurement<RangeRate>) evaluation;
126 if (rangeRateLog != null) {
127 rangeRateLog.log(ev);
128 }
129 } else if (evaluation.getObservedMeasurement() instanceof AngularAzEl) {
130 @SuppressWarnings("unchecked")
131 final EstimatedMeasurement<AngularAzEl> ev = (EstimatedMeasurement<AngularAzEl>) evaluation;
132 if (azimuthLog != null) {
133 azimuthLog.log(ev);
134 }
135 if (elevationLog != null) {
136 elevationLog.log(ev);
137 }
138 } else if (evaluation.getObservedMeasurement() instanceof Position) {
139 @SuppressWarnings("unchecked")
140 final EstimatedMeasurement<Position> ev = (EstimatedMeasurement<Position>) evaluation;
141 if (positionOnlyLog != null) {
142 positionOnlyLog.log(ev);
143 }
144 } else if (evaluation.getObservedMeasurement() instanceof PV) {
145 @SuppressWarnings("unchecked")
146 final EstimatedMeasurement<PV> ev = (EstimatedMeasurement<PV>) evaluation;
147 if (positionLog != null) {
148 positionLog.log(ev);
149 }
150 if (velocityLog != null) {
151 velocityLog.log(ev);
152 }
153 } else if (evaluation.getObservedMeasurement() instanceof MultiplexedMeasurement) {
154 for (final EstimatedMeasurement<?> em : ((MultiplexedMeasurement) evaluation.getObservedMeasurement()).getEstimatedMeasurements()) {
155 logEvaluation(em, rangeLog, rangeRateLog, azimuthLog, elevationLog, positionOnlyLog, positionLog, velocityLog);
156 }
157 }
158 }
159
160 }