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.KalmanSmoother;
20 import org.hipparchus.filtering.kalman.ProcessEstimate;
21 import org.hipparchus.linear.MatrixUtils;
22 import org.hipparchus.linear.RealMatrix;
23 import org.hipparchus.linear.RealVector;
24 import org.orekit.time.AbsoluteDate;
25 import org.orekit.utils.ParameterDriver;
26 import org.orekit.utils.ParameterDriversList;
27
28 import java.util.ArrayList;
29 import java.util.List;
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81 public class RtsSmoother implements KalmanObserver {
82
83
84 private final KalmanSmoother smoother;
85
86
87 private final AbsoluteDate referenceDate;
88
89
90 private final double[] covarianceScale;
91
92
93 private final ParameterDriversList estimatedOrbitalParameters;
94
95
96 private final ParameterDriversList estimatedPropagationParameters;
97
98
99 private final ParameterDriversList estimatedMeasurementsParameters;
100
101
102 private final List<RealVector> referenceStates;
103
104
105
106
107
108
109
110 public RtsSmoother(final AbstractKalmanEstimator estimator) {
111 this.smoother = new KalmanSmoother(estimator.getMatrixDecomposer());
112 this.referenceDate = estimator.getReferenceDate();
113 this.covarianceScale = estimator.getScale();
114 this.estimatedOrbitalParameters = estimator.getOrbitalParametersDrivers(true);
115 this.estimatedPropagationParameters = estimator.getPropagationParametersDrivers(true);
116 this.estimatedMeasurementsParameters = estimator.getEstimatedMeasurementsParameters();
117 this.referenceStates = new ArrayList<>();
118
119
120 estimator.getKalmanFilter().setObserver(smoother);
121 }
122
123
124
125 @Override
126 public void init(final KalmanEstimation estimation) {
127
128 referenceStates.add(getReferenceState());
129 }
130
131
132
133 @Override
134 public void evaluationPerformed(final KalmanEstimation estimation) {
135 referenceStates.add(getReferenceState());
136 }
137
138
139
140
141 public List<PhysicalEstimatedState> backwardsSmooth() {
142
143
144 final List<ProcessEstimate> normalisedStates = smoother.backwardsSmooth();
145
146
147 final List<PhysicalEstimatedState> smoothedStates = new ArrayList<>();
148 for (int timeIndex = 0; timeIndex < normalisedStates.size(); ++timeIndex) {
149 final ProcessEstimate normalisedState = normalisedStates.get(timeIndex);
150 final RealVector physicalState =
151 getPhysicalState(normalisedState.getState(), referenceStates.get(timeIndex));
152 final RealMatrix physicalCovariance =
153 KalmanEstimatorUtil.unnormalizeCovarianceMatrix(normalisedState.getCovariance(), covarianceScale);
154 smoothedStates.add(new PhysicalEstimatedState(
155 referenceDate.shiftedBy(normalisedState.getTime()),
156 physicalState,
157 physicalCovariance
158 ));
159 }
160 return smoothedStates;
161 }
162
163
164
165
166 private RealVector getReferenceState() {
167 final RealVector referenceState = MatrixUtils.createRealVector(covarianceScale.length);
168 int i = 0;
169 for (final ParameterDriversList.DelegatingDriver driver : estimatedOrbitalParameters.getDrivers()) {
170 referenceState.setEntry(i++, driver.getReferenceValue());
171 }
172 for (final ParameterDriversList.DelegatingDriver driver : estimatedPropagationParameters.getDrivers()) {
173 referenceState.setEntry(i++, driver.getReferenceValue());
174 }
175 for (final ParameterDriversList.DelegatingDriver driver : estimatedMeasurementsParameters.getDrivers()) {
176 referenceState.setEntry(i++, driver.getReferenceValue());
177 }
178 return referenceState;
179 }
180
181
182
183
184
185
186
187 private RealVector getPhysicalState(final RealVector normalisedState, final RealVector referenceState) {
188 final RealVector physicalState = MatrixUtils.createRealVector(covarianceScale.length);
189 int i = 0;
190 for (final ParameterDriversList.DelegatingDriver driver : estimatedOrbitalParameters.getDrivers()) {
191 physicalState.setEntry(i, setResetDriver(driver, referenceState.getEntry(i), normalisedState.getEntry(i)));
192 i += 1;
193 }
194 for (final ParameterDriversList.DelegatingDriver driver : estimatedPropagationParameters.getDrivers()) {
195 physicalState.setEntry(i, setResetDriver(driver, referenceState.getEntry(i), normalisedState.getEntry(i)));
196 i += 1;
197 }
198 for (final ParameterDriversList.DelegatingDriver driver : estimatedMeasurementsParameters.getDrivers()) {
199 physicalState.setEntry(i, setResetDriver(driver, referenceState.getEntry(i), normalisedState.getEntry(i)));
200 i += 1;
201 }
202 return physicalState;
203 }
204
205
206
207
208
209
210
211
212 private double setResetDriver(final ParameterDriver driver,
213 final double referenceValue,
214 final double normalisedVale) {
215
216 final double oldReference = driver.getReferenceValue();
217 final double oldValue = driver.getNormalizedValue();
218
219
220 driver.setReferenceValue(referenceValue);
221 driver.setNormalizedValue(normalisedVale);
222 final double physicalValue = driver.getValue();
223
224
225 driver.setReferenceValue(oldReference);
226 driver.setNormalizedValue(oldValue);
227
228 return physicalValue;
229 }
230
231 }