1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.measurements;
18
19 import java.util.IdentityHashMap;
20 import java.util.Map;
21 import java.util.stream.Stream;
22
23 import org.orekit.errors.OrekitIllegalArgumentException;
24 import org.orekit.errors.OrekitIllegalStateException;
25 import org.orekit.errors.OrekitMessages;
26 import org.orekit.propagation.SpacecraftState;
27 import org.orekit.time.AbsoluteDate;
28 import org.orekit.utils.ParameterDriver;
29 import org.orekit.utils.TimeSpanMap;
30 import org.orekit.utils.TimeStampedPVCoordinates;
31 import org.orekit.utils.TimeSpanMap.Span;
32
33
34
35
36
37
38 public class EstimatedMeasurement<T extends ObservedMeasurement<T>> extends EstimatedMeasurementBase<T> {
39
40
41 private double[][][] stateDerivatives;
42
43
44 private final Map<ParameterDriver, TimeSpanMap<double[]>> parametersDerivatives;
45
46
47
48
49
50
51
52
53
54 public EstimatedMeasurement(final T observedMeasurement,
55 final int iteration, final int count,
56 final SpacecraftState[] states,
57 final TimeStampedPVCoordinates[] participants) {
58 super(observedMeasurement, iteration, count, states, participants);
59 this.stateDerivatives = new double[states.length][][];
60 this.parametersDerivatives = new IdentityHashMap<>();
61 }
62
63
64
65
66
67 public EstimatedMeasurement(final EstimatedMeasurementBase<T> estimatedMeasurementBase) {
68 this(estimatedMeasurementBase.getObservedMeasurement(), estimatedMeasurementBase.getIteration(),
69 estimatedMeasurementBase.getCount(), estimatedMeasurementBase.getStates(),
70 estimatedMeasurementBase.getParticipants());
71 this.setEstimatedValue(estimatedMeasurementBase.getEstimatedValue());
72 this.setStatus(estimatedMeasurementBase.getStatus());
73 }
74
75
76
77
78
79
80
81
82
83 public int getStateSize() {
84 return stateDerivatives[0][0].length;
85 }
86
87
88
89
90
91
92
93
94 public double[][] getStateDerivatives(final int index) {
95 final double[][] sd = new double[getObservedMeasurement().getDimension()][];
96 for (int i = 0; i < getObservedMeasurement().getDimension(); ++i) {
97 sd[i] = stateDerivatives[index][i].clone();
98 }
99 return sd;
100 }
101
102
103
104
105
106
107
108 public void setStateDerivatives(final int index, final double[]... derivatives) {
109 this.stateDerivatives[index] = new double[getObservedMeasurement().getDimension()][];
110 for (int i = 0; i < getObservedMeasurement().getDimension(); ++i) {
111 this.stateDerivatives[index][i] = derivatives[i].clone();
112 }
113 }
114
115
116
117
118
119 public Stream<ParameterDriver> getDerivativesDrivers() {
120 return parametersDerivatives.entrySet().stream().map(Map.Entry::getKey);
121 }
122
123
124
125
126
127
128
129
130
131
132
133 public double[] getParameterDerivatives(final ParameterDriver driver)
134 throws OrekitIllegalArgumentException {
135 if (driver.getNbOfValues() == 1) {
136 final TimeSpanMap<double[]> p = parametersDerivatives.get(driver);
137 if (p == null) {
138 final StringBuilder builder = new StringBuilder();
139 for (final Map.Entry<ParameterDriver, TimeSpanMap<double[]>> entry : parametersDerivatives.entrySet()) {
140 if (builder.length() > 0) {
141 builder.append(", ");
142 }
143 builder.append(entry.getKey());
144 }
145 throw new OrekitIllegalArgumentException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME,
146 driver,
147 builder.length() > 0 ? builder.toString() : " <none>");
148 }
149 return p.get(AbsoluteDate.ARBITRARY_EPOCH);
150 } else {
151 throw new OrekitIllegalStateException(OrekitMessages.PARAMETER_WITH_SEVERAL_ESTIMATED_VALUES, driver.getName(), "getParameterDerivatives(driver, date)");
152 }
153 }
154
155
156
157
158
159
160
161
162
163 public double[] getParameterDerivatives(final ParameterDriver driver, final AbsoluteDate date)
164 throws OrekitIllegalArgumentException {
165 final TimeSpanMap<double[]> p = parametersDerivatives.get(driver);
166 if (p == null) {
167 final StringBuilder builder = new StringBuilder();
168 for (final Map.Entry<ParameterDriver, TimeSpanMap<double[]>> entry : parametersDerivatives.entrySet()) {
169 if (builder.length() > 0) {
170 builder.append(", ");
171 }
172 builder.append(entry.getKey());
173 }
174 throw new OrekitIllegalArgumentException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME,
175 driver,
176 builder.length() > 0 ? builder.toString() : "<none>");
177 }
178 return p.get(date);
179 }
180
181
182
183
184
185
186
187
188 public void setParameterDerivatives(final ParameterDriver driver, final AbsoluteDate date, final double... parameterDerivatives) {
189 if (!parametersDerivatives.containsKey(driver) || parametersDerivatives.get(driver) == null) {
190 final TimeSpanMap<double[]> derivativeSpanMap = new TimeSpanMap<>(parameterDerivatives);
191 final TimeSpanMap<String> driverNameSpan = driver.getNamesSpanMap();
192 for (Span<String> span = driverNameSpan.getSpan(driverNameSpan.getFirstSpan().getEnd()); span != null; span = span.next()) {
193 derivativeSpanMap.addValidAfter(parameterDerivatives, span.getStart(), false);
194 }
195 parametersDerivatives.put(driver, derivativeSpanMap);
196
197 } else {
198
199 AbsoluteDate dateToAddAfter = driver.getNamesSpanMap().getSpan(date).getStart();
200 if (dateToAddAfter.equals(AbsoluteDate.PAST_INFINITY)) {
201 dateToAddAfter = driver.getNamesSpanMap().getSpan(date).getEnd();
202 parametersDerivatives.get(driver).addValidBefore(parameterDerivatives, dateToAddAfter, false);
203 } else {
204 parametersDerivatives.get(driver).addValidAfter(parameterDerivatives, dateToAddAfter, false);
205 }
206
207 }
208
209 }
210
211
212
213
214
215
216 public void setParameterDerivatives(final ParameterDriver driver, final TimeSpanMap<double[]> parameterDerivativesMap) {
217 parametersDerivatives.put(driver, parameterDerivativesMap);
218 }
219
220 }