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