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 org.hipparchus.geometry.euclidean.threed.Vector3D;
20 import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem;
21 import org.orekit.estimation.leastsquares.BatchLSEstimator;
22 import org.orekit.estimation.leastsquares.BatchLSObserver;
23 import org.orekit.estimation.measurements.AngularAzEl;
24 import org.orekit.estimation.measurements.EstimatedMeasurement;
25 import org.orekit.estimation.measurements.EstimationsProvider;
26 import org.orekit.estimation.measurements.MultiplexedMeasurement;
27 import org.orekit.estimation.measurements.ObservedMeasurement;
28 import org.orekit.estimation.measurements.PV;
29 import org.orekit.estimation.measurements.Position;
30 import org.orekit.estimation.measurements.Range;
31 import org.orekit.estimation.measurements.RangeRate;
32 import org.orekit.orbits.Orbit;
33 import org.orekit.utils.PVCoordinates;
34 import org.orekit.utils.ParameterDriversList;
35
36 import java.util.Locale;
37 import java.util.Map;
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
118
119 final String measurementType = evaluation.getObservedMeasurement().getMeasurementType();
120 if (measurementType.equals(Range.MEASUREMENT_TYPE)) {
121 @SuppressWarnings("unchecked")
122 final EstimatedMeasurement<Range> ev = (EstimatedMeasurement<Range>) evaluation;
123 if (rangeLog != null) {
124 rangeLog.log(ev);
125 }
126 } else if (measurementType.equals(RangeRate.MEASUREMENT_TYPE)) {
127 @SuppressWarnings("unchecked")
128 final EstimatedMeasurement<RangeRate> ev = (EstimatedMeasurement<RangeRate>) evaluation;
129 if (rangeRateLog != null) {
130 rangeRateLog.log(ev);
131 }
132 } else if (measurementType.equals(AngularAzEl.MEASUREMENT_TYPE)) {
133 @SuppressWarnings("unchecked")
134 final EstimatedMeasurement<AngularAzEl> ev = (EstimatedMeasurement<AngularAzEl>) evaluation;
135 if (azimuthLog != null) {
136 azimuthLog.log(ev);
137 }
138 if (elevationLog != null) {
139 elevationLog.log(ev);
140 }
141 } else if (measurementType.equals(Position.MEASUREMENT_TYPE)) {
142 @SuppressWarnings("unchecked")
143 final EstimatedMeasurement<Position> ev = (EstimatedMeasurement<Position>) evaluation;
144 if (positionOnlyLog != null) {
145 positionOnlyLog.log(ev);
146 }
147 } else if (measurementType.equals(PV.MEASUREMENT_TYPE)) {
148 @SuppressWarnings("unchecked")
149 final EstimatedMeasurement<PV> ev = (EstimatedMeasurement<PV>) evaluation;
150 if (positionLog != null) {
151 positionLog.log(ev);
152 }
153 if (velocityLog != null) {
154 velocityLog.log(ev);
155 }
156 } else if (measurementType.equals(MultiplexedMeasurement.MEASUREMENT_TYPE)) {
157 for (final EstimatedMeasurement<?> em : ((MultiplexedMeasurement) evaluation.getObservedMeasurement()).getEstimatedMeasurements()) {
158 logEvaluation(em, rangeLog, rangeRateLog, azimuthLog, elevationLog, positionOnlyLog, positionLog, velocityLog);
159 }
160 }
161 }
162 }