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  package org.hipparchus.filtering.kalman;
18  
19  import org.hipparchus.exception.MathIllegalStateException;
20  import org.hipparchus.filtering.kalman.extended.ExtendedKalmanFilter;
21  import org.hipparchus.filtering.kalman.extended.NonLinearEvolution;
22  import org.hipparchus.filtering.kalman.extended.NonLinearProcess;
23  import org.hipparchus.filtering.kalman.linear.LinearEvolution;
24  import org.hipparchus.filtering.kalman.linear.LinearKalmanFilter;
25  import org.hipparchus.filtering.kalman.linear.LinearProcess;
26  import org.hipparchus.filtering.kalman.unscented.UnscentedEvolution;
27  import org.hipparchus.filtering.kalman.unscented.UnscentedKalmanFilter;
28  import org.hipparchus.filtering.kalman.unscented.UnscentedProcess;
29  import org.hipparchus.linear.*;
30  import org.hipparchus.util.MerweUnscentedTransform;
31  import org.hipparchus.util.UnscentedTransformProvider;
32  import org.junit.jupiter.api.Assertions;
33  import org.junit.jupiter.api.BeforeAll;
34  import org.junit.jupiter.api.Test;
35  
36  import java.util.Arrays;
37  import java.util.List;
38  import java.util.stream.Collectors;
39  
40  public class SmootherTest {
41  
42      private static final double PROCESS_NOISE = 0.1;
43      private static List<Reference> referenceData;
44      private static List<SimpleMeasurement> measurements;
45      private static double initialTime;
46      private static ProcessEstimate initialState;
47      private static MatrixDecomposer decomposer;
48  
49      @BeforeAll
50      static void beforeAll() {
51  
52          // Load reference data
53          referenceData = Reference.loadReferenceData(2, 1, "cv-smoother.txt");
54  
55          // Measurements (skip first one corresponding to smoothed initial state)
56          final RealMatrix measurementNoise = MatrixUtils.createRealMatrix(new double[][]{{1e-3}});
57          measurements = referenceData.stream()
58                  .skip(1)
59                  .map(r -> new SimpleMeasurement(r.getTime(), r.getZ(), measurementNoise))
60                  .collect(Collectors.toList());
61  
62          // Initial state
63          initialTime = 0.0;
64          final RealVector initialMean = MatrixUtils.createRealVector(new double[]{0.0, -0.5});
65          final RealMatrix initialCovariance = MatrixUtils.createRealMatrix(new double[][]{{0.01, 0.0}, {0.0, 0.25}});
66          initialState = new ProcessEstimate(initialTime, initialMean, initialCovariance);
67  
68          // Matrix decomposer for filter and smoother
69          decomposer = new CholeskyDecomposer(1e-15, 1e-15);
70      }
71  
72      // Test linear Kalman filter/smoother
73      // Simple linear 1D constant velocity
74      @Test
75      void testKalmanSmootherObserver() {
76  
77          // Initialise filter process
78          final LinearProcess<SimpleMeasurement> process = new LinearConstantVelocity<>(initialTime, PROCESS_NOISE);
79  
80          // Kalman filter
81          final KalmanFilter<SimpleMeasurement> filter = new LinearKalmanFilter<>(decomposer, process, initialState);
82  
83          // Smoother observer
84          final KalmanSmoother smoother = new KalmanSmoother(decomposer);
85          filter.setObserver(smoother);
86  
87          // Process measurements with filter
88          measurements.forEach(filter::estimationStep);
89  
90          // Smooth backwards
91          List<ProcessEstimate> smoothedStates = smoother.backwardsSmooth();
92  
93          // Check against reference
94          Assertions.assertEquals(referenceData.size(), smoothedStates.size());
95          for (int i = 0; i < referenceData.size(); ++i) {
96              referenceData.get(i).checkState(smoothedStates.get(i).getState(), 1e-14);
97              referenceData.get(i).checkCovariance(smoothedStates.get(i).getCovariance(), 1e-14);
98          }
99      }
100 
101 
102     // Test extended Kalman filter/smoother
103     // Simple linear 1D constant velocity
104     @Test
105     void testExtendedSmootherObserver() {
106 
107         // Initialise filter
108         final NonLinearProcess<SimpleMeasurement> process = new ExtendedConstantVelocity<>(initialTime, PROCESS_NOISE);
109 
110         // Kalman filter
111         final ExtendedKalmanFilter<SimpleMeasurement> filter = new ExtendedKalmanFilter<>(decomposer, process, initialState);
112 
113         // Smoother observer
114         final KalmanSmoother smoother = new KalmanSmoother(decomposer);
115         filter.setObserver(smoother);
116 
117         // Process measurements with filter
118         measurements.forEach(filter::estimationStep);
119 
120         // Smooth backwards
121         List<ProcessEstimate> smoothedStates = smoother.backwardsSmooth();
122 
123         // Check against reference
124         Assertions.assertEquals(referenceData.size(), smoothedStates.size());
125         for (int i = 0; i < referenceData.size(); ++i) {
126             referenceData.get(i).checkState(smoothedStates.get(i).getState(), 1e-14);
127             referenceData.get(i).checkCovariance(smoothedStates.get(i).getCovariance(), 1e-14);
128         }
129     }
130 
131 
132     // Test unscented Kalman filter/smoother
133     // Simple linear 1D constant velocity
134     @Test
135     void testUnscentedSmootherObserver() {
136 
137         // Initialise filter
138         final UnscentedProcess<SimpleMeasurement> process = new UnscentedConstantVelocity<>(initialTime, PROCESS_NOISE);
139 
140         final double alpha = 1.0;
141         final double beta = 2.0;
142         final double kappa = 0.0;
143         UnscentedTransformProvider unscentedTransformProvider =
144                 new MerweUnscentedTransform(initialState.getState().getDimension(), alpha, beta, kappa);
145 
146         final UnscentedKalmanFilter<SimpleMeasurement> filter =
147                 new UnscentedKalmanFilter<>(decomposer, process, initialState, unscentedTransformProvider);
148 
149         // Smoother observer
150         final KalmanSmoother smoother = new KalmanSmoother(decomposer);
151         filter.setObserver(smoother);
152 
153         // Process measurements with filter
154         measurements.forEach(filter::estimationStep);
155 
156         // Smooth backwards
157         List<ProcessEstimate> smoothedStates = smoother.backwardsSmooth();
158 
159         // Check against reference
160         Assertions.assertEquals(referenceData.size(), smoothedStates.size());
161         for (int i = 0; i < referenceData.size(); ++i) {
162             referenceData.get(i).checkState(smoothedStates.get(i).getState(), 1e-14);
163             referenceData.get(i).checkCovariance(smoothedStates.get(i).getCovariance(), 1e-14);
164         }
165     }
166 
167 
168     @Test
169     void testProcessMeasurements() {
170         // Initialise filter process
171         final LinearProcess<SimpleMeasurement> process = new LinearConstantVelocity<>(initialTime, PROCESS_NOISE);
172 
173         // Kalman filter
174         final KalmanFilter<SimpleMeasurement> filter = new LinearKalmanFilter<>(decomposer, process, initialState);
175 
176         // Smoother observer
177         final KalmanSmoother smoother = new KalmanSmoother(decomposer);
178         filter.setObserver(smoother);
179 
180         // Smooth backwards.  No measurements processed.  Should throw.
181         Assertions.assertThrows(MathIllegalStateException.class, smoother::backwardsSmooth);
182 
183         // Process a single measurement
184         filter.estimationStep(measurements.get(0));
185 
186         // Now should not throw
187         Assertions.assertDoesNotThrow(smoother::backwardsSmooth);
188     }
189 
190 
191     // Process model for the linear Kalman filter
192     private static class LinearConstantVelocity<T extends Measurement> implements LinearProcess<T> {
193 
194         private double currentTime;
195         private final double processNoiseScale;
196 
197         public LinearConstantVelocity(final double initialTime, final double processNoiseScale) {
198             currentTime = initialTime;
199             this.processNoiseScale = processNoiseScale;
200         }
201 
202         @Override
203         public LinearEvolution getEvolution(final T measurement) {
204             final double dt = measurement.getTime() - currentTime;
205             currentTime = measurement.getTime();
206 
207             return new LinearEvolution(
208                     getStateTransitionMatrix(dt),
209                     MatrixUtils.createRealMatrix(2, 2),
210                     MatrixUtils.createRealVector(2),
211                     getProcessNoise(dt, processNoiseScale),
212                     getMeasurementJacobian()
213             );
214         }
215     }
216 
217     // Process model for the extended Kalman filter
218     private static class ExtendedConstantVelocity<T extends Measurement> implements NonLinearProcess<T> {
219 
220         private double currentTime;
221         private final double processNoiseScale;
222 
223         public ExtendedConstantVelocity(final double initialTime, final double processNoiseScale) {
224             currentTime = initialTime;
225             this.processNoiseScale = processNoiseScale;
226         }
227 
228         @Override
229         public NonLinearEvolution getEvolution(final double previousTime,
230                                                final RealVector previousState,
231                                                final T measurement) {
232             final double dt = measurement.getTime() - currentTime;
233             currentTime = measurement.getTime();
234 
235             final RealMatrix stateTransitionMatrix = getStateTransitionMatrix(dt);
236             return new NonLinearEvolution(
237                     currentTime,
238                     stateTransitionMatrix.operate(previousState),
239                     stateTransitionMatrix,
240                     getProcessNoise(dt, processNoiseScale),
241                     getMeasurementJacobian()
242             );
243         }
244 
245         @Override
246         public RealVector getInnovation(final T measurement,
247                                         final NonLinearEvolution evolution,
248                                         final RealMatrix innovationCovarianceMatrix) {
249             return measurement.getValue().subtract(evolution.getMeasurementJacobian().operate(evolution.getCurrentState()));
250         }
251     }
252 
253     // Process model for the unscented Kalman filter
254     private static class UnscentedConstantVelocity<T extends Measurement> implements UnscentedProcess<T> {
255 
256         private double currentTime;
257         private final double processNoiseScale;
258 
259         public UnscentedConstantVelocity(final double initialTime, final double processNoiseScale) {
260             currentTime = initialTime;
261             this.processNoiseScale = processNoiseScale;
262         }
263 
264         @Override
265         public UnscentedEvolution getEvolution(final double previousTime,
266                                                final RealVector[] sigmaPoints,
267                                                final T measurement) {
268             final double dt = measurement.getTime() - currentTime;
269             currentTime = measurement.getTime();
270 
271             final RealMatrix stateTransitionMatrix = getStateTransitionMatrix(dt);
272 
273             final RealVector[] predictedSigmaPoints = Arrays.stream(sigmaPoints)
274                     .map(stateTransitionMatrix::operate)
275                     .toArray(RealVector[]::new);
276 
277             return new UnscentedEvolution(currentTime, predictedSigmaPoints);
278         }
279 
280         @Override
281         public RealMatrix getProcessNoiseMatrix(double previousTime, RealVector predictedState, T measurement) {
282             final double dt = measurement.getTime() - previousTime;
283             return getProcessNoise(dt, processNoiseScale);
284         }
285 
286         @Override
287         public RealVector[] getPredictedMeasurements(final RealVector[] predictedSigmaPoints,
288                                                      final T measurement) {
289 
290             final RealMatrix measurementJacobian = getMeasurementJacobian();
291             return Arrays.stream(predictedSigmaPoints)
292                     .map(measurementJacobian::operate)
293                     .toArray(RealVector[]::new);
294         }
295 
296         @Override
297         public RealVector getInnovation(final T measurement,
298                                         final RealVector predictedMeasurement,
299                                         final RealVector predictedState,
300                                         final RealMatrix innovationCovarianceMatrix) {
301             return measurement.getValue().subtract(predictedMeasurement);
302         }
303     }
304 
305 
306     // Common process model quantities
307     private static RealMatrix getStateTransitionMatrix(final double dt) {
308         return MatrixUtils.createRealMatrix(new double[][]{{1.0, dt}, {0.0, 1.0}});
309     }
310 
311     private static RealMatrix getProcessNoise(final double dt, final double processNoiseScale) {
312         final double dt2 = dt * dt;
313         final double dt3 = dt2 * dt;
314 
315         return MatrixUtils.createRealMatrix(new double[][]{{dt3 / 3.0, dt2 / 2.0}, {dt2 / 2.0, dt}})
316                 .scalarMultiply(processNoiseScale);
317     }
318 
319     private static RealMatrix getMeasurementJacobian() {
320         return MatrixUtils.createRealMatrix(new double[][]{{1.0, 0.0}});
321     }
322 }