1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18 package org.hipparchus.filtering.kalman.unscented;
19
20 import java.util.List;
21 import java.util.stream.IntStream;
22 import java.util.stream.Stream;
23
24 import org.hipparchus.UnitTestUtils;
25 import org.hipparchus.exception.MathIllegalArgumentException;
26 import org.hipparchus.filtering.kalman.Measurement;
27 import org.hipparchus.filtering.kalman.ProcessEstimate;
28 import org.hipparchus.filtering.kalman.Reference;
29 import org.hipparchus.filtering.kalman.SimpleMeasurement;
30 import org.hipparchus.filtering.kalman.extended.ExtendedKalmanFilter;
31 import org.hipparchus.filtering.kalman.extended.NonLinearEvolution;
32 import org.hipparchus.filtering.kalman.extended.NonLinearProcess;
33 import org.hipparchus.linear.ArrayRealVector;
34 import org.hipparchus.linear.CholeskyDecomposer;
35 import org.hipparchus.linear.MatrixUtils;
36 import org.hipparchus.linear.QRDecomposer;
37 import org.hipparchus.linear.RealMatrix;
38 import org.hipparchus.linear.RealVector;
39 import org.hipparchus.random.RandomGenerator;
40 import org.hipparchus.random.Well1024a;
41 import org.hipparchus.util.FastMath;
42 import org.hipparchus.util.MerweUnscentedTransform;
43 import org.hipparchus.util.UnscentedTransformProvider;
44 import org.junit.Assert;
45 import org.junit.Test;
46
47
48 public class UnscentedKalmanFilterTest {
49
50
51 @Test(expected = MathIllegalArgumentException.class)
52 public void testWrongStateDimension() {
53 final ConstantProcess process = new ConstantProcess();
54 final ProcessEstimate initial = new ProcessEstimate(0,
55 MatrixUtils.createRealVector(new double[0]),
56 process.q);
57 final UnscentedTransformProvider provider = new UnscentedTransformProvider() {
58
59 @Override
60 public RealVector[] unscentedTransform(RealVector state,
61 RealMatrix covariance) {
62 return new RealVector[0];
63 }
64
65 @Override
66 public RealVector getWm() {
67 return new ArrayRealVector();
68 }
69
70 @Override
71 public RealVector getWc() {
72 return new ArrayRealVector();
73 }
74 };
75
76 new UnscentedKalmanFilter<>(new CholeskyDecomposer(1.0e-15, 1.0e-15), process, initial, provider);
77 }
78
79 @Test
80 public void testConstant() {
81
82 ConstantProcess process = new ConstantProcess();
83
84
85 final ProcessEstimate initial = new ProcessEstimate(0,
86 MatrixUtils.createRealVector(new double[] { 10.0 }),
87 process.q);
88 Assert.assertNull(initial.getInnovationCovariance());
89
90
91 final List<Reference> referenceData = Reference.loadReferenceData(1, 1, "constant-value.txt");
92 final Stream<SimpleMeasurement> measurements =
93 referenceData.stream().
94 map(r -> new SimpleMeasurement(r.getTime(),
95 r.getZ(),
96 MatrixUtils.createRealDiagonalMatrix(new double[] { 0.1 })));
97
98
99 final UnscentedKalmanFilter<SimpleMeasurement> filter = new UnscentedKalmanFilter<>(new CholeskyDecomposer(1.0e-15, 1.0e-15),
100 process, initial, new MerweUnscentedTransform(initial.getState().getDimension()));
101
102
103 measurements.
104 map(measurement -> filter.estimationStep(measurement)).
105 forEach(estimate -> {
106 for (Reference r : referenceData) {
107 if (r.sameTime(estimate.getTime())) {
108 r.checkState(estimate.getState(), 1.0e-13);
109 r.checkCovariance(estimate.getCovariance(), 1.0e-15);
110 return;
111 }
112 }
113 });
114
115 }
116
117 private static class ConstantProcess implements UnscentedProcess<SimpleMeasurement> {
118
119 private RealMatrix q = MatrixUtils.createRealDiagonalMatrix(new double[] {
120 1.0e-5
121 });
122 @Override
123 public UnscentedEvolution getEvolution(double previousTime, final RealVector[] previousStateSamples, SimpleMeasurement measurement) {
124 return new UnscentedEvolution(measurement.getTime(),
125 previousStateSamples,
126 q);
127 }
128
129 @Override
130 public RealVector[] getPredictedMeasurements(RealVector[] predictedSigmaPoints, SimpleMeasurement measurement) {
131 return predictedSigmaPoints;
132 }
133
134 @Override
135 public RealVector getInnovation(SimpleMeasurement measurement, RealVector predictedMeasurement, RealVector predictedState,
136 RealMatrix innovationCovarianceMatrix) {
137 return measurement.getValue().subtract(predictedMeasurement);
138 }
139
140
141
142 }
143
144 @Test
145 public void testCannonballZeroProcessNoise() {
146 doTestCannonball(new double[][] {
147 { 0.00, 0.00, 0.00, 0.00 },
148 { 0.00, 0.00, 0.00, 0.00 },
149 { 0.00, 0.00, 0.00, 0.00 },
150 { 0.00, 0.00, 0.00, 0.00 },
151 }, "cannonball-zero-process-noise.txt",
152 1.0e-11, 2.0e-12);
153 }
154
155 @Test
156 public void testCannonballNonZeroProcessNoise() {
157 doTestCannonball(new double[][] {
158 { 0.01, 0.00, 0.00, 0.00 },
159 { 0.00, 0.10, 0.00, 0.00 },
160 { 0.00, 0.00, 0.01, 0.00 },
161 { 0.00, 0.00, 0.00, 0.10 },
162 }, "cannonball-non-zero-process-noise.txt",
163 1.0e-11, 1.0e-11);
164 }
165
166 private void doTestCannonball(final double[][] q, final String name,
167 final double tolState, final double tolCovariance) {
168 final double mNoise = 30.0;
169 final double vIni = 100.0;
170 final double alphaIni = FastMath.PI / 4;
171 final UnscentedProcess<SimpleMeasurement> process = new CannonballProcess(9.81, q);
172
173
174 final ProcessEstimate initial = new ProcessEstimate(0.0,
175 MatrixUtils.createRealVector(new double[] {
176 0.0, vIni * FastMath.cos(alphaIni),
177 0.0, vIni * FastMath.sin(alphaIni)
178 }),
179 MatrixUtils.createRealDiagonalMatrix(new double[] {
180 mNoise * mNoise, 1.0e-3, mNoise * mNoise, 1.0e-3
181 }));
182
183
184
185 final List<Reference> referenceData = Reference.loadReferenceData(4, 2, name);
186 final Stream<SimpleMeasurement> measurements =
187 referenceData.stream().
188 map(r -> new SimpleMeasurement(r.getTime(),
189 r.getZ(),
190 MatrixUtils.createRealDiagonalMatrix(new double[] {
191 mNoise * mNoise, mNoise * mNoise
192 })));
193
194
195 final UnscentedKalmanFilter<SimpleMeasurement> filter =
196 new UnscentedKalmanFilter<>(new CholeskyDecomposer(1.0e-15, 1.0e-15),
197 process, initial,new MerweUnscentedTransform(initial.getState().getDimension()));
198
199
200 measurements.
201 map(measurement -> filter.estimationStep(measurement)).
202 map(estimate -> {
203 final ProcessEstimate p = filter.getPredicted();
204 final ProcessEstimate c = filter.getCorrected();
205 Assert.assertEquals(p.getTime(), c.getTime(), 1.0e-15);
206 Assert.assertTrue(p.getState().getDistance(c.getState()) > 0.005);
207 return estimate;
208 }).
209 forEach(estimate -> {
210 for (Reference r : referenceData) {
211 if (r.sameTime(estimate.getTime())) {
212 r.checkState(estimate.getState(), tolState);
213 r.checkCovariance(estimate.getCovariance(), tolCovariance);
214 return;
215 }
216 }
217 });
218
219 }
220
221 private static class CannonballProcess implements UnscentedProcess<SimpleMeasurement> {
222 private final double g;
223 private final RealMatrix q;
224
225 public CannonballProcess(final double g, final double[][] qData) {
226 this.g = g;
227 this.q = MatrixUtils.createRealMatrix(qData);
228 }
229
230 @Override
231 public UnscentedEvolution getEvolution(double previousTime, RealVector[] previousStateSamples, SimpleMeasurement measurement) {
232 final double dt = measurement.getTime() - previousTime;
233 final RealVector[] states = new RealVector[9];
234 final RealVector[] measurementSamples = new RealVector[9];
235 for (int i = 0 ; i < 9 ; i++) {
236 states[i]= MatrixUtils.createRealVector(new double[] {previousStateSamples[i].getEntry(0) + previousStateSamples[i].getEntry(1) * dt,
237 previousStateSamples[i].getEntry(1),
238 previousStateSamples[i].getEntry(2) + previousStateSamples[i].getEntry(3) * dt - 0.5 * g * dt * dt,
239 previousStateSamples[i].getEntry(3) - g * dt});
240 measurementSamples[i]= MatrixUtils.createRealVector(new double[] { states[i].getEntry(0), states[i].getEntry(2) });
241
242 }
243
244
245
246 return new UnscentedEvolution(measurement.getTime(), states, q);
247 }
248
249 @Override
250 public RealVector[] getPredictedMeasurements(RealVector[] predictedSigmaPoints, SimpleMeasurement measurement) {
251 final RealVector[] measurementSamples = new RealVector[9];
252 for (int i = 0 ; i < 9 ; i++) {
253 measurementSamples[i]= MatrixUtils.createRealVector(new double[] { predictedSigmaPoints[i].getEntry(0),
254 predictedSigmaPoints[i].getEntry(2) });
255 }
256 return measurementSamples;
257 }
258
259 @Override
260 public RealVector getInnovation(SimpleMeasurement measurement, RealVector predictedMeasurement, RealVector predictedState,
261 RealMatrix innovationCovarianceMatrix) {
262 return measurement.getValue().subtract(predictedMeasurement);
263 }
264
265
266 }
267
268 @Test
269 public void testWelshBishopExactR() {
270 doTestWelshBishop(0xd30a8f811e2f7c61l, -0.37727, 0.1,
271 0.0, 1.0, 1.0e-5, 0.1 * 0.1,
272 50, -0.389117, 1.0e-6);
273 }
274
275 @Test
276 public void testWelshBishopBigR() {
277 doTestWelshBishop(0xd30a8f811e2f7c61l, -0.37727, 0.1,
278 0.0, 1.0, 1.0e-5, 1.0 * 1.0,
279 50, -0.385613, 1.0e-6);
280 }
281
282 @Test
283 public void testWelshBishopSmallR() {
284 doTestWelshBishop(0xd30a8f811e2f7c61l, -0.37727, 0.1,
285 0.0, 1.0, 1.0e-5, 0.01 * 0.01,
286 50, -0.403015, 1.0e-6);
287 }
288
289 private void doTestWelshBishop(final long seed,
290 final double trueConstant, final double trueStdv,
291 final double initialEstimate, final double initialCovariance,
292 final double q, final double r,
293 final int nbMeasurements,
294 final double expected, final double tolerance) {
295 WelshBishopProcess process = new WelshBishopProcess(q);
296
297
298
299
300 final ProcessEstimate initial = new ProcessEstimate(0,
301 MatrixUtils.createRealVector(new double[] { initialEstimate }),
302 MatrixUtils.createRealDiagonalMatrix(new double[] { initialCovariance }));
303 final RandomGenerator generator = new Well1024a(seed);
304 final Stream<SimpleMeasurement> measurements =
305 IntStream.
306 range(0, nbMeasurements).
307 mapToObj(i -> new SimpleMeasurement(i,
308 MatrixUtils.createRealVector(new double[] {
309 trueConstant + generator.nextGaussian() * trueStdv,
310 }),
311 MatrixUtils.createRealDiagonalMatrix(new double[] { r })));
312
313
314 final UnscentedKalmanFilter<SimpleMeasurement> filter =
315 new UnscentedKalmanFilter<>(new CholeskyDecomposer(1.0e-15, 1.0e-15),
316 process, initial, new MerweUnscentedTransform(initial.getState().getDimension()));
317
318
319 ProcessEstimate finalEstimate = measurements.
320 map(measurement -> filter.estimationStep(measurement)).
321 reduce((first, second) -> second).get();
322
323 Assert.assertEquals(expected, finalEstimate.getState().getEntry(0), tolerance);
324
325 }
326
327 private final class WelshBishopProcess implements UnscentedProcess<SimpleMeasurement> {
328
329 private RealMatrix q;
330
331 WelshBishopProcess(double qValue) {
332 q = MatrixUtils.createRealDiagonalMatrix(new double[] {
333 qValue
334 });
335 }
336 @Override
337 public UnscentedEvolution getEvolution(double previousTime,
338 RealVector[] previousStates,
339 SimpleMeasurement measurement) {
340 return new UnscentedEvolution(measurement.getTime(),
341 previousStates,
342 q);
343 }
344
345 @Override
346 public RealVector[] getPredictedMeasurements(RealVector[] predictedSigmaPoints, SimpleMeasurement measurement) {
347 return predictedSigmaPoints;
348 }
349
350 @Override
351 public RealVector getInnovation(SimpleMeasurement measurement, RealVector predictedMeasurement, RealVector predictedState,
352 RealMatrix innovationCovarianceMatrix) {
353 return measurement.getValue().subtract(predictedMeasurement);
354 }
355
356
357 }
358
359 @Test
360 public void testRadar() {
361 doTestRadar();
362 }
363
364 private void doTestRadar() {
365
366 final ProcessEstimate initial = new ProcessEstimate(0.0, MatrixUtils.createRealVector(new double[] {0., 90., 1100.}),
367 MatrixUtils.createRealMatrix(new double[][] {
368 { 100., 0.00, 0.00},
369 { 0.00, 100., 0.00},
370 { 0.00, 0.00, 100.}}));
371 final UnscentedProcess<SimpleMeasurement> process = new RadarProcess();
372 MerweUnscentedTransform utProvider = new MerweUnscentedTransform(initial.getState().getDimension(), 1.0, 2, 0 );
373 final UnscentedKalmanFilter<SimpleMeasurement> filter =
374 new UnscentedKalmanFilter<>(new CholeskyDecomposer(1.0e-15, 1.0e-15), process, initial, utProvider);
375
376
377
378 final List<Reference> referenceData = Reference.loadReferenceData(3, 1, "radar.txt");
379 final Stream<SimpleMeasurement> measurements =
380 referenceData.stream().
381 map(r -> new SimpleMeasurement(r.getTime(),
382 r.getZ(),
383 MatrixUtils.createRealDiagonalMatrix(new double[] { 10.0 })));
384
385
386 measurements.
387 map(measurement -> filter.estimationStep(measurement)).
388 forEach(estimate -> {
389 for (Reference r : referenceData) {
390 if (r.sameTime(estimate.getTime())) {
391 r.checkState(estimate.getState(), 6.0e-6);
392 r.checkCovariance(estimate.getCovariance(), 5.0e-6);
393 if (r.hasIntermediateData()) {
394 r.checkStateTransitionMatrix(estimate.getStateTransitionMatrix(), 1.0e-14);
395 r.checkMeasurementJacobian(estimate.getMeasurementJacobian(), 1.0e-15);
396 r.checkInnovationCovariance(estimate.getInnovationCovariance(), 1.0e-12);
397 r.checkKalmanGain(estimate.getKalmanGain(), 1.0e-12);
398 r.checkKalmanGain(estimate.getKalmanGain(), 1.0e-15);
399 }
400 return;
401 }
402 }
403 });
404 }
405
406
407
408
409
410
411 private static class RadarProcess implements UnscentedProcess<SimpleMeasurement> {
412
413 public RadarProcess() {
414 }
415
416
417 @Override
418 public UnscentedEvolution getEvolution(double previousTime, RealVector[] sigmaPoints, SimpleMeasurement measurement) {
419 final double dt = measurement.getTime() - previousTime;
420
421 final RealVector[] states = new RealVector[7];
422 final RealVector[] measurementSamples = new RealVector[7];
423 for (int i = 0; i < 7; i++) {
424
425 states[i]= MatrixUtils.createRealVector(new double[] {
426 sigmaPoints[i].getEntry(0) + sigmaPoints[i].getEntry(1) * dt,
427 sigmaPoints[i].getEntry(1),
428 sigmaPoints[i].getEntry(2)
429 });
430 measurementSamples[i]= MatrixUtils.createRealVector(new double[] { FastMath.sqrt(FastMath.pow(states[i].getEntry(0), 2) + FastMath.pow(states[i].getEntry(2), 2)) });
431 }
432
433
434 final RealMatrix processNoiseMatrix = MatrixUtils.createRealMatrix(new double[][] {
435 { 0.01, 0.00, 0.00},
436 { 0.00, 0.01, 0.00},
437 { 0.00, 0.00, 0.01}
438 });
439
440 return new UnscentedEvolution(measurement.getTime(), states, processNoiseMatrix);
441 }
442
443 @Override
444 public RealVector[] getPredictedMeasurements(RealVector[] predictedSigmaPoints, SimpleMeasurement measurement) {
445 final RealVector[] measurementSamples = new RealVector[7];
446 for (int i = 0; i < 7; i++) {
447 measurementSamples[i] = MatrixUtils.createRealVector(new double[] {
448 FastMath.sqrt(FastMath.pow(predictedSigmaPoints[i].getEntry(0), 2) +
449 FastMath.pow(predictedSigmaPoints[i].getEntry(2), 2))
450 });
451 }
452 return measurementSamples;
453 }
454
455
456 @Override
457 public RealVector getInnovation(SimpleMeasurement measurement, RealVector predictedMeasurement, RealVector predictedState,
458 RealMatrix innovationCovarianceMatrix) {
459 return measurement.getValue().subtract(predictedMeasurement);
460 }
461
462 }
463
464
465
466 private static final int STATE_DIMENSION = 2;
467 final static double ALPHA = 1.0;
468 final static double BETA = 2.0;
469 final static double KAPPA = 3.0 - STATE_DIMENSION;
470
471
472 static class TestMeasurement implements Measurement {
473
474 private final double time;
475 private final RealVector value;
476 private final RealMatrix covariance;
477
478 TestMeasurement(final double time, final RealVector value) {
479 this.time = time;
480 this.value = value;
481 this.covariance = MatrixUtils.createRealIdentityMatrix(value.getDimension());
482 }
483
484 @Override
485 public double getTime() {
486 return time;
487 }
488
489 @Override
490 public RealVector getValue() {
491 return value;
492 }
493
494 @Override
495 public RealMatrix getCovariance() {
496 return covariance;
497 }
498 }
499
500 static class TestEKFProcess<T extends Measurement> implements NonLinearProcess<T> {
501
502 private final double processNoiseScale;
503
504 public TestEKFProcess(final double processNoiseScale) {
505 this.processNoiseScale = processNoiseScale;
506 }
507
508 @Override
509 public NonLinearEvolution getEvolution(final double time,
510 final RealVector state,
511 T measurement) {
512
513 double dt = measurement.getTime() - time;
514
515
516 RealMatrix stateTransitionMatrix = MatrixUtils.createRealIdentityMatrix(STATE_DIMENSION);
517 stateTransitionMatrix.setEntry(0, 1, dt);
518
519
520 RealMatrix processNoiseCovariance = MatrixUtils.createRealMatrix(STATE_DIMENSION, STATE_DIMENSION);
521 processNoiseCovariance.setEntry(0, 0, dt * dt * dt / 3.0);
522 processNoiseCovariance.setEntry(0, 1, dt * dt / 2.0);
523 processNoiseCovariance.setEntry(1, 0, dt * dt / 2.0);
524 processNoiseCovariance.setEntry(1, 1, dt);
525 processNoiseCovariance = processNoiseCovariance.scalarMultiply(processNoiseScale);
526
527
528 int measurementDimension = measurement.getValue().getDimension();
529 RealMatrix measurementMatrix = MatrixUtils.createRealMatrix(measurementDimension, STATE_DIMENSION);
530 for (int row = 0; row < measurementDimension; ++row) {
531 measurementMatrix.setEntry(row, 0, 1.0);
532 }
533
534
535 RealVector predictedState = stateTransitionMatrix.operate(state);
536
537 return new NonLinearEvolution(measurement.getTime(), predictedState, stateTransitionMatrix,
538 processNoiseCovariance, measurementMatrix);
539 }
540
541 @Override
542 public RealVector getInnovation(T measurement, NonLinearEvolution evolution, RealMatrix innovationCovariance) {
543
544 return measurement.getValue().subtract(evolution.getCurrentState().getSubVector(0, 1));
545 }
546 }
547
548
549 static class TestUKFProcess<T extends Measurement> implements UnscentedProcess<T> {
550
551 private final double processNoiseScale;
552
553 public TestUKFProcess(final double processNoiseScale) {
554 this.processNoiseScale = processNoiseScale;
555 }
556
557 @Override
558 public UnscentedEvolution getEvolution(double previousTime, RealVector[] sigmaPoints, T measurement) {
559
560
561 int numPoints = sigmaPoints.length;
562 Assert.assertEquals(STATE_DIMENSION * 2 + 1, numPoints);
563
564
565 double dt = measurement.getTime() - previousTime;
566
567
568 RealVector[] predictedPoints = new RealVector[numPoints];
569 for (int i = 0; i < numPoints; ++i) {
570 double[] point = sigmaPoints[i].toArray();
571 point[0] = point[0] + dt * point[1];
572 predictedPoints[i] = MatrixUtils.createRealVector(point);
573 }
574
575
576 RealMatrix processNoiseCovariance = MatrixUtils.createRealMatrix(STATE_DIMENSION, STATE_DIMENSION);
577 processNoiseCovariance.setEntry(0, 0, dt * dt * dt / 3.0);
578 processNoiseCovariance.setEntry(0, 1, dt * dt / 2.0);
579 processNoiseCovariance.setEntry(1, 0, dt * dt / 2.0);
580 processNoiseCovariance.setEntry(1, 1, dt);
581 processNoiseCovariance = processNoiseCovariance.scalarMultiply(processNoiseScale);
582
583 return new UnscentedEvolution(measurement.getTime(), predictedPoints, processNoiseCovariance);
584 }
585
586 @Override
587 public RealVector[] getPredictedMeasurements(RealVector[] predictedSigmaPoints, T measurement) {
588 RealVector[] measurementPoints = new RealVector[predictedSigmaPoints.length];
589 for (int i = 0; i < predictedSigmaPoints.length; ++i) {
590 measurementPoints[i] = predictedSigmaPoints[i].getSubVector(0, 1);
591 }
592 return measurementPoints;
593 }
594
595 @Override
596 public RealVector getInnovation(T measurement, RealVector predictedMeasurement,
597 RealVector predictedState, RealMatrix innovationCovarianceMatrix) {
598
599 return measurement.getValue().subtract(predictedMeasurement);
600 }
601 }
602
603
604 @Test
605 public void testUkfEkfComparison() {
606
607
608 final double processNoiseScale = 1.0;
609 final double initialTime = 0.0;
610 final double[] initalState = {1.0, -0.8};
611 final double[] initialCovarianceDiagonal = {1.1, 0.2};
612 final ProcessEstimate initialEstimate = new ProcessEstimate(initialTime,
613 MatrixUtils.createRealVector(initalState),
614 MatrixUtils.createRealDiagonalMatrix(initialCovarianceDiagonal));
615
616
617 ExtendedKalmanFilter<TestMeasurement> extendedKalmanFilter = new ExtendedKalmanFilter<>(
618 new QRDecomposer(0.0),
619 new TestEKFProcess<>(processNoiseScale),
620 initialEstimate);
621
622
623 UnscentedKalmanFilter<TestMeasurement> unscentedFilter = new UnscentedKalmanFilter<>(
624 new QRDecomposer(0.0),
625 new TestUKFProcess<>(processNoiseScale),
626 initialEstimate,
627 new MerweUnscentedTransform(STATE_DIMENSION, ALPHA, BETA, KAPPA));
628
629
630 final double measurement1Time = 0.8;
631 final double[] measurement1Value = {0.3};
632 final TestMeasurement measurement1 = new TestMeasurement(measurement1Time, MatrixUtils.createRealVector(measurement1Value));
633
634
635 ProcessEstimate ekfEstimate = extendedKalmanFilter.estimationStep(measurement1);
636 ProcessEstimate ukfEstimate = unscentedFilter.estimationStep(measurement1);
637
638
639 UnitTestUtils.assertEquals("EKF and UKF states",
640 ekfEstimate.getState(), ukfEstimate.getState(), 1e-15);
641 UnitTestUtils.assertEquals("EKF and UKF covariances",
642 ekfEstimate.getCovariance(), ukfEstimate.getCovariance(), 1e-15);
643
644 }
645
646 }