-
Notifications
You must be signed in to change notification settings - Fork 141
/
KalmanFilter.java
executable file
·235 lines (198 loc) · 7.12 KB
/
KalmanFilter.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
package org.lst.trading.main.strategy.kalman;
import org.la4j.LinearAlgebra;
import org.la4j.Matrix;
import org.la4j.matrix.DenseMatrix;
/**
* n: Number of states
* m: Number of sensors
* <p>
* https://www.udacity.com/wiki/cs373/kalman-filter-matrices
* <p>
* Steps:
* <p>
* PREDICT X'
* <p>
* First, we predict our next x:
* <p>
* x' = Fx + u
* <p>
* UPDATE P'
* <p>
* We also update the covariance matrix according to the next prediction:
* <p>
* P' = FP(transp F)
* <p>
* UPDATE Y
* <p>
* y becomes the difference between the move and what we expected:
* <p>
* y = z - Hx
* <p>
* UPDATE S
* <p>
* S is the covariance of the move, adding up the covariance in move space of the position and the covariance of the measurement:
* <p>
* S = HP(transp H) + R
* <p>
* CALCULATE K
* <p>
* Now I start to wave my hands. I assume this next matrix is called K because this is the work of the Kalman filter. Whatever is happening here, it doesn't depend on u or z. We're computing how much of the difference between the observed move and the expected move to add to x.
* <p>
* K = P (transp H) (inv S)
* <p>
* UPDATE X'
* <p>
* We update x:
* <p>
* x' = x + Ky
* <p>
* SUBTRACT P
* <p>
* And we subtract some uncertainty from P, again not depending on u or z:
* <p>
* P' = P - P(transp H)(inv S)HP
*/
public class KalmanFilter {
private final int mStateCount; // n
private final int mSensorCount; // m
// state
/**
* stateCount x 1
*/
private Matrix mState; // x, state estimate
/**
* stateCount x stateCount
* <p>
* Symmetric.
* Down the diagonal of P, we find the variances of the elements of x.
* On the off diagonals, at P[i][j], we find the covariances of x[i] with x[j].
*/
private Matrix mStateCovariance; // Covariance matrix of x, process noise (w)
// predict
/**
* stateCount x stateCount
* <p>
* Kalman filters model a system over time.
* After each tick of time, we predict what the values of x are, and then we measure and do some computation.
* F is used in the update step. Here's how it works: For each value in x, we write an equation to update that value,
* a linear equation in all the variables in x. Then we can just read off the coefficients to make the matrix.
*/
private Matrix mUpdateMatrix; // F, State transition matrix.
/**
* stateCount x stateCount
* <p>
* Error in the process, after each update this uncertainty is added.
*/
private Matrix mUpdateCovariance; // Q, Estimated error in process.
/**
* stateCount x 1
* <p>
* The control input, the move vector.
* It's the change to x that we cause, or that we know is happening.
* Since we add it to x, it has dimension n. When the filter updates, it adds u to the new x.
* <p>
* External moves to the system.
*/
private Matrix mMoveVector; // u, Control vector
// measurement
/**
* sensorCount x 1
* <p>
* z: Measurement Vector, It's the outputs from the sensors.
*/
private Matrix mMeasurement;
/**
* sensorCount x sensorCount
* <p>
* R, the variances and covariances of our sensor measurements.
* <p>
* The Kalman filter algorithm does not change R, because the process can't change our belief about the
* accuracy of our sensors--that's a property of the sensors themselves.
* We know the variance of our sensor either by testing it, or by reading the documentation that came with it,
* or something like that. Note that the covariances here are the covariances of the measurement error.
* A positive number means that if the first sensor is erroneously low, the second tends to be erroneously low,
* or if the first reads high, the second tends to read high; it doesn't mean that if the first sensor reports a
* high number the second will also report a high number
*/
private Matrix mMeasurementCovariance; // R, Covariance matrix of the measurement vector z
/**
* sensorCount x stateCount
* <p>
* The matrix H tells us what sensor readings we'd get if x were the true state of affairs and our sensors were perfect.
* It's the matrix we use to extract the measurement from the data.
* If we multiply H times a perfectly correct x, we get a perfectly correct z.
*/
private Matrix mExtractionMatrix; // H, Observation matrix.
// no inputs
private Matrix mInnovation;
private Matrix mInnovationCovariance;
public KalmanFilter(int stateCount, int sensorCount) {
mStateCount = stateCount;
mSensorCount = sensorCount;
mMoveVector = Matrix.zero(stateCount, 1);
}
private void step() {
// prediction
Matrix predictedState = mUpdateMatrix.multiply(mState).add(mMoveVector);
Matrix predictedStateCovariance = mUpdateMatrix.multiply(mStateCovariance).multiply(mUpdateMatrix.transpose()).add(mUpdateCovariance);
// observation
mInnovation = mMeasurement.subtract(mExtractionMatrix.multiply(predictedState));
mInnovationCovariance = mExtractionMatrix.multiply(predictedStateCovariance).multiply(mExtractionMatrix.transpose()).add(mMeasurementCovariance);
// update
Matrix kalmanGain = predictedStateCovariance.multiply(mExtractionMatrix.transpose()).multiply(mInnovationCovariance.withInverter(LinearAlgebra.InverterFactory.SMART).inverse());
mState = predictedState.add(kalmanGain.multiply(mInnovation));
int nRow = mStateCovariance.rows();
mStateCovariance = DenseMatrix.identity(nRow).subtract(kalmanGain.multiply(mExtractionMatrix)).multiply(predictedStateCovariance);
}
public void step(Matrix measurement, Matrix move) {
mMeasurement = measurement;
mMoveVector = move;
step();
}
public void step(Matrix measurement) {
mMeasurement = measurement;
step();
}
public Matrix getState() {
return mState;
}
public Matrix getStateCovariance() {
return mStateCovariance;
}
public Matrix getInnovation() {
return mInnovation;
}
public Matrix getInnovationCovariance() {
return mInnovationCovariance;
}
public void setState(Matrix state) {
mState = state;
}
public void setStateCovariance(Matrix stateCovariance) {
mStateCovariance = stateCovariance;
}
public void setUpdateMatrix(Matrix updateMatrix) {
mUpdateMatrix = updateMatrix;
}
public void setUpdateCovariance(Matrix updateCovariance) {
mUpdateCovariance = updateCovariance;
}
public void setMeasurementCovariance(Matrix measurementCovariance) {
mMeasurementCovariance = measurementCovariance;
}
public void setExtractionMatrix(Matrix h) {
this.mExtractionMatrix = h;
}
public Matrix getUpdateMatrix() {
return mUpdateMatrix;
}
public Matrix getUpdateCovariance() {
return mUpdateCovariance;
}
public Matrix getMeasurementCovariance() {
return mMeasurementCovariance;
}
public Matrix getExtractionMatrix() {
return mExtractionMatrix;
}
}