1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.hipparchus.filtering.kalman;
18
19 import org.hipparchus.exception.MathIllegalStateException;
20 import org.hipparchus.filtering.kalman.extended.ExtendedKalmanFilter;
21 import org.hipparchus.filtering.kalman.extended.NonLinearEvolution;
22 import org.hipparchus.filtering.kalman.extended.NonLinearProcess;
23 import org.hipparchus.filtering.kalman.linear.LinearEvolution;
24 import org.hipparchus.filtering.kalman.linear.LinearKalmanFilter;
25 import org.hipparchus.filtering.kalman.linear.LinearProcess;
26 import org.hipparchus.filtering.kalman.unscented.UnscentedEvolution;
27 import org.hipparchus.filtering.kalman.unscented.UnscentedKalmanFilter;
28 import org.hipparchus.filtering.kalman.unscented.UnscentedProcess;
29 import org.hipparchus.linear.*;
30 import org.hipparchus.util.MerweUnscentedTransform;
31 import org.hipparchus.util.UnscentedTransformProvider;
32 import org.junit.jupiter.api.Assertions;
33 import org.junit.jupiter.api.BeforeAll;
34 import org.junit.jupiter.api.Test;
35
36 import java.util.Arrays;
37 import java.util.List;
38 import java.util.stream.Collectors;
39
40 public class SmootherTest {
41
42 private static final double PROCESS_NOISE = 0.1;
43 private static List<Reference> referenceData;
44 private static List<SimpleMeasurement> measurements;
45 private static double initialTime;
46 private static ProcessEstimate initialState;
47 private static MatrixDecomposer decomposer;
48
49 @BeforeAll
50 static void beforeAll() {
51
52
53 referenceData = Reference.loadReferenceData(2, 1, "cv-smoother.txt");
54
55
56 final RealMatrix measurementNoise = MatrixUtils.createRealMatrix(new double[][]{{1e-3}});
57 measurements = referenceData.stream()
58 .skip(1)
59 .map(r -> new SimpleMeasurement(r.getTime(), r.getZ(), measurementNoise))
60 .collect(Collectors.toList());
61
62
63 initialTime = 0.0;
64 final RealVector initialMean = MatrixUtils.createRealVector(new double[]{0.0, -0.5});
65 final RealMatrix initialCovariance = MatrixUtils.createRealMatrix(new double[][]{{0.01, 0.0}, {0.0, 0.25}});
66 initialState = new ProcessEstimate(initialTime, initialMean, initialCovariance);
67
68
69 decomposer = new CholeskyDecomposer(1e-15, 1e-15);
70 }
71
72
73
74 @Test
75 void testKalmanSmootherObserver() {
76
77
78 final LinearProcess<SimpleMeasurement> process = new LinearConstantVelocity<>(initialTime, PROCESS_NOISE);
79
80
81 final KalmanFilter<SimpleMeasurement> filter = new LinearKalmanFilter<>(decomposer, process, initialState);
82
83
84 final KalmanSmoother smoother = new KalmanSmoother(decomposer);
85 filter.setObserver(smoother);
86
87
88 measurements.forEach(filter::estimationStep);
89
90
91 List<ProcessEstimate> smoothedStates = smoother.backwardsSmooth();
92
93
94 Assertions.assertEquals(referenceData.size(), smoothedStates.size());
95 for (int i = 0; i < referenceData.size(); ++i) {
96 referenceData.get(i).checkState(smoothedStates.get(i).getState(), 1e-14);
97 referenceData.get(i).checkCovariance(smoothedStates.get(i).getCovariance(), 1e-14);
98 }
99 }
100
101
102
103
104 @Test
105 void testExtendedSmootherObserver() {
106
107
108 final NonLinearProcess<SimpleMeasurement> process = new ExtendedConstantVelocity<>(initialTime, PROCESS_NOISE);
109
110
111 final ExtendedKalmanFilter<SimpleMeasurement> filter = new ExtendedKalmanFilter<>(decomposer, process, initialState);
112
113
114 final KalmanSmoother smoother = new KalmanSmoother(decomposer);
115 filter.setObserver(smoother);
116
117
118 measurements.forEach(filter::estimationStep);
119
120
121 List<ProcessEstimate> smoothedStates = smoother.backwardsSmooth();
122
123
124 Assertions.assertEquals(referenceData.size(), smoothedStates.size());
125 for (int i = 0; i < referenceData.size(); ++i) {
126 referenceData.get(i).checkState(smoothedStates.get(i).getState(), 1e-14);
127 referenceData.get(i).checkCovariance(smoothedStates.get(i).getCovariance(), 1e-14);
128 }
129 }
130
131
132
133
134 @Test
135 void testUnscentedSmootherObserver() {
136
137
138 final UnscentedProcess<SimpleMeasurement> process = new UnscentedConstantVelocity<>(initialTime, PROCESS_NOISE);
139
140 final double alpha = 1.0;
141 final double beta = 2.0;
142 final double kappa = 0.0;
143 UnscentedTransformProvider unscentedTransformProvider =
144 new MerweUnscentedTransform(initialState.getState().getDimension(), alpha, beta, kappa);
145
146 final UnscentedKalmanFilter<SimpleMeasurement> filter =
147 new UnscentedKalmanFilter<>(decomposer, process, initialState, unscentedTransformProvider);
148
149
150 final KalmanSmoother smoother = new KalmanSmoother(decomposer);
151 filter.setObserver(smoother);
152
153
154 measurements.forEach(filter::estimationStep);
155
156
157 List<ProcessEstimate> smoothedStates = smoother.backwardsSmooth();
158
159
160 Assertions.assertEquals(referenceData.size(), smoothedStates.size());
161 for (int i = 0; i < referenceData.size(); ++i) {
162 referenceData.get(i).checkState(smoothedStates.get(i).getState(), 1e-14);
163 referenceData.get(i).checkCovariance(smoothedStates.get(i).getCovariance(), 1e-14);
164 }
165 }
166
167
168 @Test
169 void testProcessMeasurements() {
170
171 final LinearProcess<SimpleMeasurement> process = new LinearConstantVelocity<>(initialTime, PROCESS_NOISE);
172
173
174 final KalmanFilter<SimpleMeasurement> filter = new LinearKalmanFilter<>(decomposer, process, initialState);
175
176
177 final KalmanSmoother smoother = new KalmanSmoother(decomposer);
178 filter.setObserver(smoother);
179
180
181 Assertions.assertThrows(MathIllegalStateException.class, smoother::backwardsSmooth);
182
183
184 filter.estimationStep(measurements.get(0));
185
186
187 Assertions.assertDoesNotThrow(smoother::backwardsSmooth);
188 }
189
190
191
192 private static class LinearConstantVelocity<T extends Measurement> implements LinearProcess<T> {
193
194 private double currentTime;
195 private final double processNoiseScale;
196
197 public LinearConstantVelocity(final double initialTime, final double processNoiseScale) {
198 currentTime = initialTime;
199 this.processNoiseScale = processNoiseScale;
200 }
201
202 @Override
203 public LinearEvolution getEvolution(final T measurement) {
204 final double dt = measurement.getTime() - currentTime;
205 currentTime = measurement.getTime();
206
207 return new LinearEvolution(
208 getStateTransitionMatrix(dt),
209 MatrixUtils.createRealMatrix(2, 2),
210 MatrixUtils.createRealVector(2),
211 getProcessNoise(dt, processNoiseScale),
212 getMeasurementJacobian()
213 );
214 }
215 }
216
217
218 private static class ExtendedConstantVelocity<T extends Measurement> implements NonLinearProcess<T> {
219
220 private double currentTime;
221 private final double processNoiseScale;
222
223 public ExtendedConstantVelocity(final double initialTime, final double processNoiseScale) {
224 currentTime = initialTime;
225 this.processNoiseScale = processNoiseScale;
226 }
227
228 @Override
229 public NonLinearEvolution getEvolution(final double previousTime,
230 final RealVector previousState,
231 final T measurement) {
232 final double dt = measurement.getTime() - currentTime;
233 currentTime = measurement.getTime();
234
235 final RealMatrix stateTransitionMatrix = getStateTransitionMatrix(dt);
236 return new NonLinearEvolution(
237 currentTime,
238 stateTransitionMatrix.operate(previousState),
239 stateTransitionMatrix,
240 getProcessNoise(dt, processNoiseScale),
241 getMeasurementJacobian()
242 );
243 }
244
245 @Override
246 public RealVector getInnovation(final T measurement,
247 final NonLinearEvolution evolution,
248 final RealMatrix innovationCovarianceMatrix) {
249 return measurement.getValue().subtract(evolution.getMeasurementJacobian().operate(evolution.getCurrentState()));
250 }
251 }
252
253
254 private static class UnscentedConstantVelocity<T extends Measurement> implements UnscentedProcess<T> {
255
256 private double currentTime;
257 private final double processNoiseScale;
258
259 public UnscentedConstantVelocity(final double initialTime, final double processNoiseScale) {
260 currentTime = initialTime;
261 this.processNoiseScale = processNoiseScale;
262 }
263
264 @Override
265 public UnscentedEvolution getEvolution(final double previousTime,
266 final RealVector[] sigmaPoints,
267 final T measurement) {
268 final double dt = measurement.getTime() - currentTime;
269 currentTime = measurement.getTime();
270
271 final RealMatrix stateTransitionMatrix = getStateTransitionMatrix(dt);
272
273 final RealVector[] predictedSigmaPoints = Arrays.stream(sigmaPoints)
274 .map(stateTransitionMatrix::operate)
275 .toArray(RealVector[]::new);
276
277 return new UnscentedEvolution(currentTime, predictedSigmaPoints);
278 }
279
280 @Override
281 public RealMatrix getProcessNoiseMatrix(double previousTime, RealVector predictedState, T measurement) {
282 final double dt = measurement.getTime() - previousTime;
283 return getProcessNoise(dt, processNoiseScale);
284 }
285
286 @Override
287 public RealVector[] getPredictedMeasurements(final RealVector[] predictedSigmaPoints,
288 final T measurement) {
289
290 final RealMatrix measurementJacobian = getMeasurementJacobian();
291 return Arrays.stream(predictedSigmaPoints)
292 .map(measurementJacobian::operate)
293 .toArray(RealVector[]::new);
294 }
295
296 @Override
297 public RealVector getInnovation(final T measurement,
298 final RealVector predictedMeasurement,
299 final RealVector predictedState,
300 final RealMatrix innovationCovarianceMatrix) {
301 return measurement.getValue().subtract(predictedMeasurement);
302 }
303 }
304
305
306
307 private static RealMatrix getStateTransitionMatrix(final double dt) {
308 return MatrixUtils.createRealMatrix(new double[][]{{1.0, dt}, {0.0, 1.0}});
309 }
310
311 private static RealMatrix getProcessNoise(final double dt, final double processNoiseScale) {
312 final double dt2 = dt * dt;
313 final double dt3 = dt2 * dt;
314
315 return MatrixUtils.createRealMatrix(new double[][]{{dt3 / 3.0, dt2 / 2.0}, {dt2 / 2.0, dt}})
316 .scalarMultiply(processNoiseScale);
317 }
318
319 private static RealMatrix getMeasurementJacobian() {
320 return MatrixUtils.createRealMatrix(new double[][]{{1.0, 0.0}});
321 }
322 }