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