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