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 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
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 }