1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18 package org.hipparchus.filtering.kalman.extended;
19
20 import org.hipparchus.filtering.kalman.ProcessEstimate;
21 import org.hipparchus.filtering.kalman.Reference;
22 import org.hipparchus.filtering.kalman.SimpleMeasurement;
23 import org.hipparchus.linear.CholeskyDecomposer;
24 import org.hipparchus.linear.MatrixUtils;
25 import org.hipparchus.linear.RealMatrix;
26 import org.hipparchus.linear.RealVector;
27 import org.hipparchus.random.RandomGenerator;
28 import org.hipparchus.random.Well1024a;
29 import org.hipparchus.util.FastMath;
30 import org.junit.jupiter.api.Test;
31
32 import java.util.List;
33 import java.util.stream.IntStream;
34 import java.util.stream.Stream;
35
36 import static org.junit.jupiter.api.Assertions.assertEquals;
37 import static org.junit.jupiter.api.Assertions.assertNull;
38 import static org.junit.jupiter.api.Assertions.assertTrue;
39
40 class ExtendedKalmanFilterTest {
41
42 @Test
43 void testConstant() {
44
45 ConstantProcess process = new ConstantProcess();
46
47
48 final ProcessEstimate initial = new ProcessEstimate(0,
49 MatrixUtils.createRealVector(new double[] { 10.0 }),
50 process.q);
51 assertNull(initial.getInnovationCovariance());
52
53
54 final List<Reference> referenceData = Reference.loadReferenceData(1, 1, "constant-value.txt");
55 final Stream<SimpleMeasurement> measurements =
56 referenceData.stream().
57 map(r -> new SimpleMeasurement(r.getTime(),
58 r.getZ(),
59 MatrixUtils.createRealDiagonalMatrix(new double[] { 0.1 })));
60
61
62 final ExtendedKalmanFilter<SimpleMeasurement> filter =
63 new ExtendedKalmanFilter<>(new CholeskyDecomposer(1.0e-15, 1.0e-15),
64 process, initial);
65
66
67 measurements.
68 map(measurement -> filter.estimationStep(measurement)).
69 forEach(estimate -> {
70 for (Reference r : referenceData) {
71 if (r.sameTime(estimate.getTime())) {
72 r.checkState(estimate.getState(), 1.0e-15);
73 r.checkCovariance(estimate.getCovariance(), 3.0e-19);
74 return;
75 }
76 }
77 });
78
79 }
80
81 private static class ConstantProcess implements NonLinearProcess<SimpleMeasurement> {
82
83 private RealMatrix q = MatrixUtils.createRealDiagonalMatrix(new double[] {
84 1.0e-5
85 });
86
87 @Override
88 public NonLinearEvolution getEvolution(double previousTime, RealVector previousState, SimpleMeasurement measurement) {
89 return new NonLinearEvolution(measurement.getTime(),
90 previousState,
91 MatrixUtils.createRealIdentityMatrix(1),
92 q,
93 MatrixUtils.createRealMatrix(new double[][] { { 1.0 } }));
94 }
95
96 @Override
97 public RealVector getInnovation(SimpleMeasurement measurement, NonLinearEvolution evolution, RealMatrix innovationCovarianceMatrix) {
98 return measurement.getValue().subtract(evolution.getCurrentState());
99 }
100
101 }
102
103 @Test
104 void testConstantAcceleration() {
105 doTestConstantAcceleration("constant-acceleration.txt");
106 }
107
108 @Test
109 void testConstantAccelerationWithIntermediateData() {
110 doTestConstantAcceleration("constant-acceleration-with-intermediate-data.txt");
111 }
112
113 @Test
114 void testConstantAccelerationWithOutlier() {
115 doTestConstantAcceleration("constant-acceleration-with-outlier.txt");
116 }
117
118 private void doTestConstantAcceleration(String name) {
119
120 final double acc = 0.1;
121 final double aNoise = 0.2;
122 final double mNoise = 10.0;
123 final NonLinearProcess<SimpleMeasurement> process = new ConstantAccelerationProcess(acc, aNoise);
124
125
126 final ProcessEstimate initial = new ProcessEstimate(0,
127 MatrixUtils.createRealVector(new double[] { 0.0, 0.0 }),
128 MatrixUtils.createRealMatrix(new double[][] {
129 { 1.0, 1.0 },
130 { 1.0, 1.0 }
131 }));
132
133
134 final List<Reference> referenceData = Reference.loadReferenceData(2, 1, name);
135 final Stream<SimpleMeasurement> measurements =
136 referenceData.stream().
137 map(r -> new SimpleMeasurement(r.getTime(),
138 r.getZ(),
139 MatrixUtils.createRealDiagonalMatrix(new double[] { mNoise * mNoise })));
140
141
142 final ExtendedKalmanFilter<SimpleMeasurement> filter =
143 new ExtendedKalmanFilter<>(new CholeskyDecomposer(1.0e-15, 1.0e-15), process, initial);
144
145
146 measurements.
147 map(measurement -> filter.estimationStep(measurement)).
148 forEach(estimate -> {
149 for (Reference r : referenceData) {
150 if (r.sameTime(estimate.getTime())) {
151 r.checkState(estimate.getState(), 6.0e-15);
152 r.checkCovariance(estimate.getCovariance(), 5.0e-15);
153 if (r.hasIntermediateData()) {
154 r.checkStateTransitionMatrix(estimate.getStateTransitionMatrix(), 1.0e-14);
155 r.checkMeasurementJacobian(estimate.getMeasurementJacobian(), 1.0e-15);
156 r.checkInnovationCovariance(estimate.getInnovationCovariance(), 1.0e-12);
157 r.checkKalmanGain(estimate.getKalmanGain(), 1.0e-12);
158 r.checkKalmanGain(estimate.getKalmanGain(), 1.0e-15);
159 }
160 return;
161 }
162 }
163 });
164
165 }
166
167 private static class ConstantAccelerationProcess implements NonLinearProcess<SimpleMeasurement> {
168 private final double acc;
169 private final double aNoise2;
170
171 public ConstantAccelerationProcess(final double acc, final double aNoise) {
172 this.acc = acc;
173 this.aNoise2 = aNoise * aNoise;
174 }
175
176 @Override
177 public NonLinearEvolution getEvolution(double previousTime, RealVector previousState, SimpleMeasurement measurement) {
178 final double dt = measurement.getTime() - previousTime;
179 final double dt2 = dt * dt;
180 final double dt3 = dt2 * dt;
181 final double dt4 = dt2 * dt2;
182 final RealVector state = MatrixUtils.createRealVector(new double[] {
183 previousState.getEntry(0) + previousState.getEntry(1) * dt + 0.5 * acc * dt * dt,
184 previousState.getEntry(1) + acc * dt
185 });
186 final RealMatrix stm = MatrixUtils.createRealMatrix(new double[][] {
187 { 1.0, dt },
188 { 0.0, 1.0 }
189 });
190 final RealMatrix processNoiseMatrix = MatrixUtils.createRealMatrix(new double[][] {
191 { 0.25 * dt4 * aNoise2, 0.5 * dt3 * aNoise2 },
192 { 0.5 * dt3 * aNoise2, dt2 * aNoise2 }
193 });
194 RealMatrix h = (measurement.getValue().getEntry(0) > 1.0e6) ?
195 null : MatrixUtils.createRealMatrix(new double[][] { { 1.0, 0.0 } });
196 return new NonLinearEvolution(measurement.getTime(), state, stm, processNoiseMatrix, h);
197 }
198
199 @Override
200 public RealVector getInnovation(SimpleMeasurement measurement,
201 NonLinearEvolution evolution,
202 RealMatrix innovationCovarianceMatrix) {
203 return measurement.getValue().subtract(evolution.getCurrentState().getSubVector(0, 1));
204 }
205
206 }
207
208 @Test
209 void testCannonballZeroProcessNoise() {
210 doTestCannonball(new double[][] {
211 { 0.00, 0.00, 0.00, 0.00 },
212 { 0.00, 0.00, 0.00, 0.00 },
213 { 0.00, 0.00, 0.00, 0.00 },
214 { 0.00, 0.00, 0.00, 0.00 },
215 }, "cannonball-zero-process-noise.txt",
216 5.0e-13, 6.0e-14);
217 }
218
219 @Test
220 void testCannonballNonZeroProcessNoise() {
221 doTestCannonball(new double[][] {
222 { 0.01, 0.00, 0.00, 0.00 },
223 { 0.00, 0.10, 0.00, 0.00 },
224 { 0.00, 0.00, 0.01, 0.00 },
225 { 0.00, 0.00, 0.00, 0.10 },
226 }, "cannonball-non-zero-process-noise.txt",
227 4.0e-13, 2.0e-13);
228 }
229
230 private void doTestCannonball(final double[][] q, final String name,
231 final double tolState, final double tolCovariance) {
232
233 final double mNoise = 30.0;
234 final double vIni = 100.0;
235 final double alphaIni = FastMath.PI / 4;
236 final NonLinearProcess<SimpleMeasurement> process = new CannonballProcess(9.81, q);
237
238
239 final ProcessEstimate initial = new ProcessEstimate(0.0,
240 MatrixUtils.createRealVector(new double[] {
241 0.0, vIni * FastMath.cos(alphaIni),
242 0.0, vIni * FastMath.sin(alphaIni)
243 }),
244 MatrixUtils.createRealDiagonalMatrix(new double[] {
245 mNoise * mNoise, 1.0e-3, mNoise * mNoise, 1.0e-3
246 }));
247
248
249
250 final List<Reference> referenceData = Reference.loadReferenceData(4, 2, name);
251 final Stream<SimpleMeasurement> measurements =
252 referenceData.stream().
253 map(r -> new SimpleMeasurement(r.getTime(),
254 r.getZ(),
255 MatrixUtils.createRealDiagonalMatrix(new double[] {
256 mNoise * mNoise, mNoise * mNoise
257 })));
258
259
260 final ExtendedKalmanFilter<SimpleMeasurement> filter =
261 new ExtendedKalmanFilter<>(new CholeskyDecomposer(1.0e-15, 1.0e-15), process, initial);
262
263
264 measurements.
265 map(measurement -> filter.estimationStep(measurement)).
266 map(estimate -> {
267 final ProcessEstimate p = filter.getPredicted();
268 final ProcessEstimate c = filter.getCorrected();
269 assertEquals(p.getTime(), c.getTime(), 1.0e-15);
270 assertTrue(p.getState().getDistance(c.getState()) > 0.005);
271 return estimate;
272 }).
273 forEach(estimate -> {
274 for (Reference r : referenceData) {
275 if (r.sameTime(estimate.getTime())) {
276 r.checkState(estimate.getState(), tolState);
277 r.checkCovariance(estimate.getCovariance(), tolCovariance);
278 return;
279 }
280 }
281 });
282
283 }
284
285 private static class CannonballProcess implements NonLinearProcess<SimpleMeasurement> {
286 private final double g;
287 private final RealMatrix q;
288
289 public CannonballProcess(final double g, final double[][] qData) {
290 this.g = g;
291 this.q = MatrixUtils.createRealMatrix(qData);
292 }
293
294 @Override
295 public NonLinearEvolution getEvolution(double previousTime, RealVector previousState, SimpleMeasurement measurement) {
296 final double dt = measurement.getTime() - previousTime;
297 final RealVector state = MatrixUtils.createRealVector(new double[] {
298 previousState.getEntry(0) + previousState.getEntry(1) * dt,
299 previousState.getEntry(1),
300 previousState.getEntry(2) + previousState.getEntry(3) * dt - 0.5 * g * dt * dt,
301 previousState.getEntry(3) - g * dt
302 });
303 final RealMatrix stm = MatrixUtils.createRealMatrix(new double[][] {
304 { 1.0, dt, 0.0, 0.0 },
305 { 0.0, 1.0, 0.0, 0.0 },
306 { 0.0, 0.0, 1.0, dt },
307 { 0.0, 0.0, 0.0, 1.0 },
308 });
309 return new NonLinearEvolution(measurement.getTime(), state, stm, q,
310 MatrixUtils.createRealMatrix(new double[][] {
311 { 1.0, 0.0, 0.0, 0.0 },
312 { 0.0, 0.0, 1.0, 0.0 }
313 }));
314 }
315
316 @Override
317 public RealVector getInnovation(SimpleMeasurement measurement, NonLinearEvolution evolution,
318 RealMatrix innovationCovarianceMatrix) {
319 return measurement.getValue().
320 subtract(MatrixUtils.createRealVector(new double[] {
321 evolution.getCurrentState().getEntry(0),
322 evolution.getCurrentState().getEntry(2)
323 }));
324 }
325
326 }
327
328 @Test
329 void testWelshBishopExactR() {
330 doTestWelshBishop(0xd30a8f811e2f7c61l, -0.37727, 0.1,
331 0.0, 1.0, 1.0e-5, 0.1 * 0.1,
332 50, -0.389117, 1.0e-6);
333 }
334
335 @Test
336 void testWelshBishopBigR() {
337 doTestWelshBishop(0xd30a8f811e2f7c61l, -0.37727, 0.1,
338 0.0, 1.0, 1.0e-5, 1.0 * 1.0,
339 50, -0.385613, 1.0e-6);
340 }
341
342 @Test
343 void testWelshBishopSmallR() {
344 doTestWelshBishop(0xd30a8f811e2f7c61l, -0.37727, 0.1,
345 0.0, 1.0, 1.0e-5, 0.01 * 0.01,
346 50, -0.403015, 1.0e-6);
347 }
348
349 private void doTestWelshBishop(final long seed,
350 final double trueConstant, final double trueStdv,
351 final double initialEstimate, final double initialCovariance,
352 final double q, final double r,
353 final int nbMeasurements,
354 final double expected, final double tolerance) {
355
356 WelshBishopProcess process = new WelshBishopProcess(q);
357
358
359
360
361 final ProcessEstimate initial = new ProcessEstimate(0,
362 MatrixUtils.createRealVector(new double[] { initialEstimate }),
363 MatrixUtils.createRealDiagonalMatrix(new double[] { initialCovariance }));
364 final RandomGenerator generator = new Well1024a(seed);
365 final Stream<SimpleMeasurement> measurements =
366 IntStream.
367 range(0, nbMeasurements).
368 mapToObj(i -> new SimpleMeasurement(i,
369 MatrixUtils.createRealVector(new double[] {
370 trueConstant + generator.nextGaussian() * trueStdv,
371 }),
372 MatrixUtils.createRealDiagonalMatrix(new double[] { r })));
373
374
375 final ExtendedKalmanFilter<SimpleMeasurement> filter =
376 new ExtendedKalmanFilter<>(new CholeskyDecomposer(1.0e-15, 1.0e-15),
377 process, initial);
378
379
380 ProcessEstimate finalEstimate = measurements.
381 map(measurement -> filter.estimationStep(measurement)).
382 reduce((first, second) -> second).get();
383
384 assertEquals(expected, finalEstimate.getState().getEntry(0), tolerance);
385
386 }
387
388 private final class WelshBishopProcess implements NonLinearProcess<SimpleMeasurement> {
389
390 private RealMatrix q;
391
392 WelshBishopProcess(double qValue) {
393 q = MatrixUtils.createRealDiagonalMatrix(new double[] {
394 qValue
395 });
396 }
397 @Override
398 public NonLinearEvolution getEvolution(double previousTime,
399 RealVector previousState,
400 SimpleMeasurement measurement) {
401 return new NonLinearEvolution(measurement.getTime(),
402 previousState,
403 MatrixUtils.createRealIdentityMatrix(1),
404 q,
405 MatrixUtils.createRealMatrix(new double[][] { { 1.0 } }));
406 }
407
408 @Override
409 public RealVector getInnovation(SimpleMeasurement measurement,
410 NonLinearEvolution evolution,
411 RealMatrix innovationCovarianceMatrix) {
412 return measurement.getValue().subtract(evolution.getCurrentState());
413 }
414
415 }
416
417 }