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;
19  
20  import org.hipparchus.linear.MatrixUtils;
21  import org.hipparchus.linear.RealMatrix;
22  import org.hipparchus.linear.RealVector;
23  import org.hipparchus.util.FastMath;
24  
25  import java.io.BufferedReader;
26  import java.io.IOException;
27  import java.io.InputStream;
28  import java.io.InputStreamReader;
29  import java.util.ArrayList;
30  import java.util.List;
31  
32  import static org.junit.jupiter.api.Assertions.assertEquals;
33  import static org.junit.jupiter.api.Assertions.fail;
34  
35  public class Reference {
36  
37      private final double     time;
38      private final RealVector z;
39      private final RealVector s;
40      private final RealMatrix c;
41      private final RealMatrix stm;
42      private final RealMatrix h;
43      private final RealMatrix icm;
44      private final RealMatrix k;
45  
46      private Reference(final int stateDimension, final int measurementDimension, final String line) {
47          final String[] fields = line.split("\\s+");
48          int index = 0;
49          time = Double.parseDouble(fields[index++]);
50          z = MatrixUtils.createRealVector(new double[measurementDimension]);
51          for (int i = 0; i < measurementDimension; ++i) {
52              z.setEntry(i, Double.parseDouble(fields[index++]));
53          }
54          s = MatrixUtils.createRealVector(new double[stateDimension]);
55          for (int i = 0; i < stateDimension; ++i) {
56              s.setEntry(i, Double.parseDouble(fields[index++]));
57          }
58          c = MatrixUtils.createRealMatrix(stateDimension, stateDimension);
59          for (int i = 0; i < stateDimension; ++i) {
60              for (int j = i; j < stateDimension; ++j) {
61                  c.setEntry(i, j, Double.parseDouble(fields[index++]));
62                  c.setEntry(j, i, c.getEntry(i, j));
63              }
64          }
65          if (fields.length > index) {
66              // there are additional data
67  
68              stm = MatrixUtils.createRealMatrix(stateDimension, stateDimension);
69              for (int i = 0; i < stateDimension; ++i) {
70                  for (int j = 0; j < stateDimension; ++j) {
71                      stm.setEntry(i, j, Double.parseDouble(fields[index++]));
72                  }
73              }
74  
75              h = MatrixUtils.createRealMatrix(measurementDimension, stateDimension);
76              for (int i = 0; i < measurementDimension; ++i) {
77                  for (int j = 0; j < stateDimension; ++j) {
78                      h.setEntry(i, j, Double.parseDouble(fields[index++]));
79                  }
80              }
81  
82              icm = MatrixUtils.createRealMatrix(measurementDimension, measurementDimension);
83              for (int i = 0; i < measurementDimension; ++i) {
84                  for (int j = i; j < measurementDimension; ++j) {
85                      icm.setEntry(i, j, Double.parseDouble(fields[index++]));
86                      icm.setEntry(j, i, icm.getEntry(i, j));
87                  }
88              }
89  
90              k = MatrixUtils.createRealMatrix(stateDimension, measurementDimension);
91              for (int i = 0; i < stateDimension; ++i) {
92                  for (int j = 0; j < measurementDimension; ++j) {
93                      k.setEntry(i, j, Double.parseDouble(fields[index++]));
94                  }
95              }
96  
97          } else {
98              stm = null;
99              h   = null;
100             icm = null;
101             k   = null;
102         }
103     }
104 
105     public static List<Reference> loadReferenceData(final int stateDimension, final int measurementDimension,
106                                                     final String name) {
107         List<Reference> loaded = new ArrayList<>();
108         try (InputStream is = KalmanFilter.class.getResourceAsStream(name);
109              InputStreamReader isr = new InputStreamReader(is, "UTF-8");
110              BufferedReader br = new BufferedReader(isr)) {
111             for (String line = br.readLine(); line != null; line = br.readLine()) {
112                 line = line.trim();
113                 if (line.length() > 0 && !line.startsWith("#")) {
114                     loaded.add(new Reference(stateDimension, measurementDimension, line));
115                 }
116             }
117         } catch (IOException ioe) {
118             fail(ioe.getLocalizedMessage());
119         }
120         return loaded;
121     }
122 
123     public boolean sameTime(final double otherTime) {
124         return FastMath.abs(time - otherTime) < 1.0e-6;
125     }
126 
127     public void checkState(final RealVector otherState, final double tolerance) {
128         checkVector(s, otherState, tolerance);
129     }
130 
131     public void checkCovariance(final RealMatrix otherCovariance, final double tolerance) {
132         checkMatrix(c, otherCovariance, tolerance);
133     }
134 
135     public boolean hasIntermediateData() {
136         return stm != null;
137     }
138 
139     public void checkStateTransitionMatrix(final RealMatrix otherSTM, final double tolerance) {
140         checkMatrix(stm, otherSTM, tolerance);
141     }
142 
143     public void checkMeasurementJacobian(final RealMatrix otherMeasurementJacobian, final double tolerance) {
144         checkMatrix(h, otherMeasurementJacobian, tolerance);
145     }
146 
147     public void checkInnovationCovariance(final RealMatrix otherInnovationCovariance, final double tolerance) {
148         checkMatrix(icm, otherInnovationCovariance, tolerance);
149     }
150 
151     public void checkKalmanGain(final RealMatrix otherKalmanGain, final double tolerance) {
152         checkMatrix(k, otherKalmanGain, tolerance);
153     }
154 
155     public double getTime() {
156         return time;
157     }
158 
159     public RealVector getZ() {
160         return z;
161     }
162 
163     private void checkVector(final RealVector referenceVector, final RealVector otherVector, final double tolerance) {
164         assertEquals(referenceVector.getDimension(), otherVector.getDimension());
165         for (int i = 0; i < referenceVector.getDimension(); ++i) {
166             assertEquals(referenceVector.getEntry(i), otherVector.getEntry(i), tolerance, time + ": ");
167         }
168     }
169 
170     private void checkMatrix(final RealMatrix referenceMatrix, final RealMatrix otherMatrix, final double tolerance) {
171         assertEquals(referenceMatrix.getRowDimension(), otherMatrix.getRowDimension());
172         assertEquals(referenceMatrix.getColumnDimension(), otherMatrix.getColumnDimension());
173         for (int i = 0; i < referenceMatrix.getRowDimension(); ++i) {
174             for (int j = i; j < referenceMatrix.getColumnDimension(); ++j) {
175                 assertEquals(referenceMatrix.getEntry(i, j), otherMatrix.getEntry(i, j), tolerance, time + ": ");
176             }
177         }
178     }
179 
180 }