1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
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
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 }