View Javadoc
1   /*
2    * Licensed to the Hipparchus project under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * The Hipparchus project licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *      https://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
16   */
17  
18  package org.hipparchus.filtering.kalman.linear;
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 LinearKalmanFilterTest {
38  
39      @Test
40      public void testConstant() {
41          final RealMatrix a = MatrixUtils.createRealIdentityMatrix(1);
42          final RealMatrix b = null;
43          final RealVector u = null;
44          final RealMatrix q = MatrixUtils.createRealDiagonalMatrix(new double[] {
45                                                                        1.0e-5
46                                                                    });
47  
48          // initial estimate is perfect, and process noise is perfectly known
49          final ProcessEstimate initial = new ProcessEstimate(0,
50                                                              MatrixUtils.createRealVector(new double[] { 10.0 }),
51                                                              q);
52          Assert.assertNull(initial.getInnovationCovariance());
53  
54          // reference values from Apache Commons Math 3.6.1 unit test
55          final List<Reference> referenceData = Reference.loadReferenceData(1, 1, "constant-value.txt");
56          final Stream<SimpleMeasurement> measurements =
57                          referenceData.stream().
58                          map(r -> new SimpleMeasurement(r.getTime(),
59                                                         r.getZ(),
60                                                         MatrixUtils.createRealDiagonalMatrix(new double[] { 0.1 })));
61  
62          // set up Kalman filter
63          final LinearKalmanFilter<SimpleMeasurement> filter =
64                          new LinearKalmanFilter<>(new CholeskyDecomposer(1.0e-15, 1.0e-15),
65                                                 measurement -> new LinearEvolution(a, b, u, q,
66                                                                                    MatrixUtils.createRealMatrix(new double[][] { { 1.0 } })),
67                                                 initial);
68  
69          // sequentially process all measurements and check against the reference estimated state and covariance
70          measurements.
71          map(measurement -> filter.estimationStep(measurement)).
72          forEach(estimate -> {
73              for (Reference r : referenceData) {
74                  if (r.sameTime(estimate.getTime())) {
75                      r.checkState(estimate.getState(), 1.0e-15);
76                      r.checkCovariance(estimate.getCovariance(), 3.0e-19);
77                      return;
78                  }
79              }
80          });
81  
82      }
83  
84      @Test
85      public void testConstantAcceleration() {
86          doTestConstantAcceleration("constant-acceleration.txt");
87      }
88  
89      @Test
90      public void testConstantAccelerationWithIntermediateData() {
91          doTestConstantAcceleration("constant-acceleration-with-intermediate-data.txt");
92      }
93  
94      @Test
95      public void testConstantAccelerationWithOutlier() {
96          doTestConstantAcceleration("constant-acceleration-with-outlier.txt");
97      }
98  
99      private void doTestConstantAcceleration(String name) {
100 
101         // state:             { position, velocity }
102         // control:           0.1 m/s² acceleration
103         // process noise:     induced by 0.2 m/s² acceleration noise
104         // measurement:       on position only
105         // measurement noise: 10 m (big!)
106 
107         final double dt      = 0.1;
108         final double dt2     = dt  * dt;
109         final double dt3     = dt2 * dt;
110         final double dt4     = dt2 * dt2;
111         final double acc     = 0.1;
112         final double aNoise  = 0.2;
113         final double aNoise2 = aNoise * aNoise;
114         final double mNoise  = 10.0;
115         final RealMatrix a = MatrixUtils.createRealMatrix(new double[][] {
116             { 1.0, dt },
117             { 0.0, 1.0 }
118         });
119         final RealMatrix b = MatrixUtils.createRealMatrix(new double[][] {
120             { 0.5 * dt2 },
121             { dt }
122         });
123         final RealVector u = MatrixUtils.createRealVector(new double[] { acc });
124         final RealMatrix q = MatrixUtils.createRealMatrix(new double[][] {
125             { 0.25 * dt4 * aNoise2, 0.5 * dt3 * aNoise2 },
126             { 0.5  * dt3 * aNoise2, dt2 * aNoise2 }
127         });
128 
129         // initial state is estimated to be at rest on origin
130         final ProcessEstimate initial = new ProcessEstimate(0,
131                                                             MatrixUtils.createRealVector(new double[] { 0.0, 0.0 }),
132                                                             MatrixUtils.createRealMatrix(new double[][] {
133                                                                 { 1.0, 1.0 },
134                                                                 { 1.0, 1.0 }
135                                                             }));
136 
137         // reference values from Apache Commons Math 3.6.1 unit test
138         // possibly with additional intermediate data
139         final List<Reference> referenceData = Reference.loadReferenceData(2, 1, name);
140         final Stream<SimpleMeasurement> measurements =
141                         referenceData.stream().
142                         map(r -> new SimpleMeasurement(r.getTime(),
143                                                        r.getZ(),
144                                                        MatrixUtils.createRealDiagonalMatrix(new double[] { mNoise * mNoise })));
145 
146         // set up Kalman filter
147         final LinearKalmanFilter<SimpleMeasurement> filter =
148         new LinearKalmanFilter<>(new CholeskyDecomposer(1.0e-15, 1.0e-15),
149                                measurement -> {
150                                    RealMatrix h = (measurement.getValue().getEntry(0) > 1.0e6) ?
151                                                   null :
152                                                   MatrixUtils.createRealMatrix(new double[][] { { 1.0, 0.0 } });
153                                    return new LinearEvolution(a, b, u, q, h);
154                                },
155                                initial);
156 
157         // sequentially process all measurements and check against the reference estimate
158         measurements.
159         map(measurement -> filter.estimationStep(measurement)).
160         forEach(estimate -> {
161             for (Reference r : referenceData) {
162                 if (r.sameTime(estimate.getTime())) {
163                     r.checkState(estimate.getState(), 4.0e-15);
164                     r.checkCovariance(estimate.getCovariance(), 4.0e-15);
165                     if (r.hasIntermediateData()) {
166                       r.checkStateTransitionMatrix(estimate.getStateTransitionMatrix(), 1.0e-14);
167                       r.checkMeasurementJacobian(estimate.getMeasurementJacobian(),     1.0e-15);
168                       r.checkInnovationCovariance(estimate.getInnovationCovariance(),   1.0e-12);
169                       r.checkKalmanGain(estimate.getKalmanGain(),                       1.0e-12);
170                       r.checkKalmanGain(estimate.getKalmanGain(),                       1.0e-15);
171                     }
172                     return;
173                 }
174             }
175         });
176 
177     }
178 
179     @Test
180     public void testCannonballZeroProcessNoise() {
181         doTestCannonball(new double[][] {
182                             { 0.00, 0.00, 0.00, 0.00 },
183                             { 0.00, 0.00, 0.00, 0.00 },
184                             { 0.00, 0.00, 0.00, 0.00 },
185                             { 0.00, 0.00, 0.00, 0.00 },
186                          }, "cannonball-zero-process-noise.txt",
187                          9.0e-16, 6.0e-14);
188     }
189 
190     @Test
191     public void testCannonballNonZeroProcessNoise() {
192         doTestCannonball(new double[][] {
193                             { 0.01, 0.00, 0.00, 0.00 },
194                             { 0.00, 0.10, 0.00, 0.00 },
195                             { 0.00, 0.00, 0.01, 0.00 },
196                             { 0.00, 0.00, 0.00, 0.10 },
197                          }, "cannonball-non-zero-process-noise.txt",
198                          2.0e-13, 2.0e-13);
199     }
200 
201     private void doTestCannonball(final double[][] qData, final String name,
202                                   final double tolState, final double tolCovariance) {
203 
204         final double dt       = 0.1;
205         final double g        = 9.81;
206         final double mNoise   = 30.0;
207         final double vIni     = 100.0;
208         final double alphaIni = FastMath.PI / 4;
209         final RealMatrix a = MatrixUtils.createRealMatrix(new double[][] {
210             { 1.0,  dt, 0.0, 0.0 },
211             { 0.0, 1.0, 0.0, 0.0 },
212             { 0.0, 0.0, 1.0,  dt },
213             { 0.0, 0.0, 0.0, 1.0 },
214         });
215         final RealMatrix b = MatrixUtils.createRealMatrix(new double[][] {
216             { 0.0, 0.0 },
217             { 0.0, 0.0 },
218             { 1.0, 0.0 },
219             { 0.0, 1.0 }
220         });
221         final RealVector u = MatrixUtils.createRealVector(new double[] {
222             -0.5 * g * dt * dt, -g * dt
223         });
224         final RealMatrix q = MatrixUtils.createRealMatrix(qData);
225 
226         // initial state is estimated to be a shot from origin with known angle and velocity
227         final ProcessEstimate initial = new ProcessEstimate(0,
228                                                             MatrixUtils.createRealVector(new double[] {
229                                                                  0.0, vIni * FastMath.cos(alphaIni),
230                                                                  0.0, vIni * FastMath.sin(alphaIni)
231                                                             }),
232                                                             MatrixUtils.createRealDiagonalMatrix(new double[] {
233                                                                 mNoise * mNoise, 1.0e-3, mNoise * mNoise, 1.0e-3
234                                                             }));
235 
236         // reference values from Apache Commons Math 3.6.1 unit test
237         // we have changed the test slightly, setting up a non-zero process noise
238         final List<Reference> referenceData = Reference.loadReferenceData(4, 2, name);
239         final Stream<SimpleMeasurement> measurements =
240                         referenceData.stream().
241                         map(r -> new SimpleMeasurement(r.getTime(),
242                                                        r.getZ(),
243                                                        MatrixUtils.createRealDiagonalMatrix(new double[] {
244                                                            mNoise * mNoise, mNoise * mNoise
245                                                        })));
246 
247         // set up Kalman filter
248         final LinearKalmanFilter<SimpleMeasurement> filter =
249         new LinearKalmanFilter<>(new CholeskyDecomposer(1.0e-15, 1.0e-15),
250                                   time -> new LinearEvolution(a, b, u, q,
251                                                               MatrixUtils.createRealMatrix(new double[][] {
252                                                                   { 1.0, 0.0, 0.0, 0.0 },
253                                                                   { 0.0, 0.0, 1.0, 0.0 }
254                                                               })),
255                                   initial);
256 
257         // sequentially process all measurements and check against the reference estimate
258         measurements.
259         map(measurement -> filter.estimationStep(measurement)).
260         map(estimate -> {
261             final ProcessEstimate p = filter.getPredicted();
262             final ProcessEstimate c = filter.getCorrected();
263             Assert.assertEquals(p.getTime(), c.getTime(), 1.0e-15);
264             Assert.assertTrue(p.getState().getDistance(c.getState()) > 0.005);
265             return estimate;
266         }).
267         forEach(estimate -> {
268             for (Reference r : referenceData) {
269                 if (r.sameTime(estimate.getTime())) {
270                     r.checkState(estimate.getState(), tolState);
271                     r.checkCovariance(estimate.getCovariance(), tolCovariance);
272                     return;
273                 }
274             }
275         });
276 
277     }
278 
279     @Test
280     public void testWelshBishopExactR() {
281         doTestWelshBishop(0xd30a8f811e2f7c61l, -0.37727, 0.1,
282                           0.0, 1.0, 1.0e-5, 0.1 * 0.1,
283                           50, -0.389117, 1.0e-6);
284     }
285 
286     @Test
287     public void testWelshBishopBigR() {
288         doTestWelshBishop(0xd30a8f811e2f7c61l, -0.37727, 0.1,
289                           0.0, 1.0, 1.0e-5, 1.0 * 1.0,
290                           50, -0.385613, 1.0e-6);
291     }
292 
293     @Test
294     public void testWelshBishopSmallR() {
295         doTestWelshBishop(0xd30a8f811e2f7c61l, -0.37727, 0.1,
296                           0.0, 1.0, 1.0e-5, 0.01 * 0.01,
297                           50, -0.403015, 1.0e-6);
298     }
299 
300     private void doTestWelshBishop(final long seed,
301                                    final double trueConstant, final double trueStdv,
302                                    final double initialEstimate, final double initialCovariance,
303                                    final double qValue, final double r,
304                                    final int nbMeasurements,
305                                    final double expected, final double tolerance) {
306 
307         // this is the constant voltage example from paper
308         // An Introduction to the Kalman Filter, Greg Welch and Gary Bishop
309         // available from http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
310         final RealMatrix a = MatrixUtils.createRealIdentityMatrix(1);
311         final RealMatrix b = null;
312         final RealVector u = null;
313         final RealMatrix q = MatrixUtils.createRealDiagonalMatrix(new double[] {
314             qValue
315         });
316         final ProcessEstimate initial = new ProcessEstimate(0,
317                                                       MatrixUtils.createRealVector(new double[] { initialEstimate }),
318                                                       MatrixUtils.createRealDiagonalMatrix(new double[] { initialCovariance }));
319         final RandomGenerator generator = new Well1024a(seed);
320         final Stream<SimpleMeasurement> measurements =
321                         IntStream.
322                         range(0, nbMeasurements).
323                         mapToObj(i -> new SimpleMeasurement(i,
324                                                             MatrixUtils.createRealVector(new double[] {
325                                                                 trueConstant + generator.nextGaussian() * trueStdv,
326                                                             }),
327                                                             MatrixUtils.createRealDiagonalMatrix(new double[] { r })));
328 
329         // set up Kalman filter
330         final LinearKalmanFilter<SimpleMeasurement> filter =
331                         new LinearKalmanFilter<>(new CholeskyDecomposer(1.0e-15, 1.0e-15),
332                                                measurement -> new LinearEvolution(a, b, u, q,
333                                                                                   MatrixUtils.createRealMatrix(new double[][] { { 1.0 } })),
334                                                initial);
335 
336         // sequentially process all measurements and get only the last one
337         final ProcessEstimate finalEstimate = measurements.
338                         map(measurement -> filter.estimationStep(measurement)).
339                         reduce((first, second) -> second).get();
340 
341         Assert.assertEquals(expected, finalEstimate.getState().getEntry(0), tolerance);
342 
343     }
344 
345 }