Merge lp:~oontvoo/kalmanfilterimpl/runnable_class into lp:kalmanfilterimpl
- runnable_class
- Merge into trunk
Proposed by
Vy Nguyen
Status: | Needs review |
---|---|
Proposed branch: | lp:~oontvoo/kalmanfilterimpl/runnable_class |
Merge into: | lp:kalmanfilterimpl |
Diff against target: |
723 lines (+588/-86) 6 files modified
.bzrignore (+2/-0) src/kalmanfilter/RunFilter.java (+13/-1) src/kalmanfilter/RunFilter2.java (+355/-0) src/kalmanfilter/impl/KalmanFilter.java (+33/-85) src/kalmanfilter/impl/KalmanFilter1.java (+84/-0) src/kalmanfilter/impl/KalmanFilter2.java (+101/-0) |
To merge this branch: | bzr merge lp:~oontvoo/kalmanfilterimpl/runnable_class |
Related bugs: |
Reviewer | Review Type | Date Requested | Status |
---|---|---|---|
Vy Thuy Nguyen | Pending | ||
Review via email: mp+113821@code.launchpad.net |
Commit message
Description of the change
- Refactor KalmanFilter into an interface.
- Add a runnable class that could generate/read data from text file which can then be used (by Excel) to graph the data
Probably needs A LOT of fixings. The graph doesn't look that good.
To post a comment you must log in.
Unmerged revisions
- 2. By vnguyen (<email address hidden>)
-
create a runnable class
Preview Diff
[H/L] Next/Prev Comment, [J/K] Next/Prev File, [N/P] Next/Prev Hunk
1 | === added file '.bzrignore' |
2 | --- .bzrignore 1970-01-01 00:00:00 +0000 |
3 | +++ .bzrignore 2012-07-07 07:30:27 +0000 |
4 | @@ -0,0 +1,2 @@ |
5 | +nbproject/** |
6 | +build/** |
7 | |
8 | === modified file 'src/kalmanfilter/RunFilter.java' |
9 | --- src/kalmanfilter/RunFilter.java 2012-07-03 18:46:12 +0000 |
10 | +++ src/kalmanfilter/RunFilter.java 2012-07-07 07:30:27 +0000 |
11 | @@ -26,6 +26,8 @@ |
12 | import java.util.Arrays; |
13 | import java.util.Collections; |
14 | import java.util.List; |
15 | +import kalmanfilter.impl.KalmanFilter1; |
16 | +import kalmanfilter.impl.KalmanFilter2; |
17 | |
18 | /** |
19 | * |
20 | @@ -67,6 +69,16 @@ |
21 | this.ay = ay; |
22 | } |
23 | |
24 | + public String toString() |
25 | + { |
26 | + return "(" + x + ", " |
27 | + + y + ", " |
28 | + + vx + ", " |
29 | + + vy + ", " |
30 | + + ax + ", " |
31 | + + ay + ", " |
32 | + + t + ")"; |
33 | + } |
34 | @Override |
35 | public int compareTo(Point o) |
36 | { |
37 | @@ -88,7 +100,7 @@ |
38 | List<Point> output = new ArrayList<Point>(inputs.size()); |
39 | |
40 | //(Matrix X, Matrix P, Matrix Q, Matrix H, Matrix A, Matrix R) |
41 | - KalmanFilter filter = new KalmanFilter(createX(inputs, 0), |
42 | + KalmanFilter2 filter = new KalmanFilter2(createX(inputs, 0), |
43 | getP0(), |
44 | getQ(), |
45 | getH(), |
46 | |
47 | === added file 'src/kalmanfilter/RunFilter2.java' |
48 | --- src/kalmanfilter/RunFilter2.java 1970-01-01 00:00:00 +0000 |
49 | +++ src/kalmanfilter/RunFilter2.java 2012-07-07 07:30:27 +0000 |
50 | @@ -0,0 +1,355 @@ |
51 | +/** |
52 | + * Implementation of the Kalman Filter |
53 | + * Copyright (C) 2012 Vy Nguyen |
54 | + * |
55 | + * This library is free software; you can redistribute it and/or |
56 | + * modify it under the terms of the GNU Library General Public |
57 | + * License as published by the Free Software Foundation; either |
58 | + * version 2 of the License, or (at your option) any later version. |
59 | + * |
60 | + * This library is distributed in the hope that it will be useful, |
61 | + * but WITHOUT ANY WARRANTY; without even the implied warranty of |
62 | + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
63 | + * Library General Public License for more details. |
64 | + * |
65 | + * You should have received a copy of the GNU Library General Public |
66 | + * License along with this library; if not, write to the |
67 | + * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor, |
68 | + * Boston, MA 02110-1301, USA. |
69 | + */ |
70 | + |
71 | +package kalmanfilter; |
72 | + |
73 | +import Jama.Matrix; |
74 | +import java.io.*; |
75 | +import java.util.*; |
76 | +import kalmanfilter.RunFilter.Point; |
77 | +import kalmanfilter.impl.KalmanFilter; |
78 | +import kalmanfilter.impl.KalmanFilter1; |
79 | + |
80 | +/** |
81 | + * |
82 | + * @author Vy Thao Nguyen |
83 | + */ |
84 | +public class RunFilter2 |
85 | +{ |
86 | + private static List<Point> createIdealValues(int count, double dt, |
87 | + double ax0, double ay0, |
88 | + double vx0, double vy0, |
89 | + double x0, double y0) |
90 | + { |
91 | + // x = x0 + v * dt |
92 | + // v = v0 + a * dt |
93 | + |
94 | + List<Point> ret = new ArrayList<Point>(); |
95 | + ret.add(new Point(x0, y0, 0, vx0, vy0, ax0, ay0)); // first point |
96 | + --count; |
97 | + |
98 | + double t = dt; |
99 | + for (int n = 0; n < count; ++n) |
100 | + { |
101 | + double vx = vx0 + ax0 * t; |
102 | + double x = x0 + vx0 * t + 0.5 * ay0 * t * t; |
103 | + |
104 | + double vy = vy0 + ay0 * t; |
105 | + double y = y0 + vy0 * t + 0.5 * ay0 * t * t; |
106 | + |
107 | + ret.add(new Point(x, y, t, vx, vy, ax0, ay0)); |
108 | + t += dt; |
109 | + } |
110 | + return ret; |
111 | + } |
112 | + |
113 | + private static void writeToFile(String fileName, List<Point> toWrite) throws IOException |
114 | + { |
115 | + StringBuilder bd = new StringBuilder(); |
116 | + |
117 | + for (Point p : toWrite) |
118 | + bd.append(p.x).append(" ").append(p.y).append(System.getProperty("line.separator")); |
119 | + |
120 | + FileWriter fstream = new FileWriter(fileName); |
121 | + BufferedWriter out = new BufferedWriter(fstream); |
122 | + out.write(bd.toString()); |
123 | + out.close(); |
124 | + } |
125 | + |
126 | + private static double parseArg(String args[]) |
127 | + { |
128 | + if (args.length != 1) |
129 | + { |
130 | + System.out.println("default noise: " + DEFAULT_NOISE); |
131 | + return DEFAULT_NOISE; |
132 | + } |
133 | + return Double.parseDouble(args[0]); |
134 | + } |
135 | + |
136 | + private static List<Point> readSensorData(double dt, double ax) |
137 | + { |
138 | + try |
139 | + { |
140 | + Scanner input = new Scanner(new File(SENSOR)); |
141 | + double t = 0; |
142 | + List<Point> ret = new ArrayList<Point>(); |
143 | + |
144 | + while(input.hasNext()) |
145 | + { |
146 | + double x = Double.parseDouble(input.next()); |
147 | + double y = Double.parseDouble(input.next()); |
148 | + ret.add(new Point(x, y, t, 0, 0, ax, ax)); |
149 | + t += dt; |
150 | + } |
151 | + return ret; |
152 | + } |
153 | + catch (FileNotFoundException ex) |
154 | + { |
155 | + System.out.println("File not found: " + SENSOR); |
156 | + System.exit(2); |
157 | + } |
158 | + return null; |
159 | + } |
160 | + |
161 | + public static void main (String args[]) throws IOException |
162 | + { |
163 | + double ax = 2.5; |
164 | + double ay = 1.256; |
165 | + double dt = 0.8; |
166 | + |
167 | + System.out.println("computing the ideal values"); |
168 | + List<Point> idealData = createIdealValues(9, dt, |
169 | + ax, ay , |
170 | + 0, 0, |
171 | + 0, 0); |
172 | + writeToFile(IDEAL, idealData); |
173 | + |
174 | + System.out.println("reading sensor data"); |
175 | + List<Point> sensor = readSensorData(dt, ax); |
176 | + double noise = parseArg(args); |
177 | + |
178 | + System.out.println("filtering sensor data"); |
179 | + writeToFile(CORRECTED, filter(sensor, noise)); |
180 | + } |
181 | + /** |
182 | + * |
183 | + * @param inputs |
184 | + * @param sensorNoise |
185 | + * @return |
186 | + */ |
187 | + public static List<Point> filter(List<Point> inputs, |
188 | + double sensorNoise) |
189 | + { |
190 | + // sort the points in time order |
191 | + Collections.sort(inputs); |
192 | + |
193 | + // compute the velocity |
194 | + List<Point> updated = computeVAndA(inputs); |
195 | + |
196 | + // get the standard deviation of acceleration |
197 | + double sd = getSd(updated); |
198 | + |
199 | + List<Point> output = new ArrayList<Point>(inputs.size()); |
200 | + output.add(inputs.get(0)); // first point is the same |
201 | + |
202 | + Matrix P0 = getP0(); |
203 | + Matrix H = getH(); |
204 | + Matrix R = createR(sensorNoise); |
205 | + |
206 | + KalmanFilter filterX = new KalmanFilter1(createXx(updated.get(0)), |
207 | + P0, |
208 | + H, |
209 | + R |
210 | + ); |
211 | + |
212 | + KalmanFilter filterY = new KalmanFilter1(createXy(updated.get(0)), |
213 | + P0, |
214 | + H, |
215 | + R |
216 | + ); |
217 | + for (int n = 1; n < updated.size(); ++n) |
218 | + { |
219 | + Point cur = updated.get(n); |
220 | + Matrix A = createA(cur); |
221 | + Matrix G = createG(cur); |
222 | + Matrix Gx = G.times(cur.ax); |
223 | + Matrix Gy = G.times(cur.ay); |
224 | + Matrix Q = getQ(G, sd); |
225 | + |
226 | + Matrix correctedX = filterX.timeUpdate(A, Q, Gx).messurementUpdate(createXx(cur)); |
227 | + Matrix correctedY = filterY.timeUpdate(A, Q, Gy).messurementUpdate(createXy(cur)); |
228 | + |
229 | + output.add(decodeXY(correctedX, correctedY, inputs.get(n))); |
230 | + } |
231 | + |
232 | + return output; |
233 | + } |
234 | + private static double getSd(List<Point> p) |
235 | + { |
236 | + // for now, the accelerations are const |
237 | + return 0; |
238 | + } |
239 | + |
240 | + /** |
241 | + * TODO: correctly decode the state matrix into a point. |
242 | + * (that is, retrieve a and v) |
243 | + * |
244 | + * @param X the corrected state matrix |
245 | + * @return the corrected value of (x,y,t) |
246 | + */ |
247 | + private static Point decodeXY(Matrix X, Matrix Y, Point input) |
248 | + { |
249 | + return new Point(X.get(0, 0) / 10, // x |
250 | + Y.get(0, 0) / 10, // y |
251 | + X.get(1, 0) / 10, // vx |
252 | + Y.get(1, 0) / 10, // vy |
253 | + input.ax, // unchanged |
254 | + input.ay, // unchanged |
255 | + input.t // unchanged |
256 | + ); |
257 | + } |
258 | + |
259 | + /** |
260 | + * TODO: correctly encode the point (velocity, acceleration, etc.) into a matrix. |
261 | + * |
262 | + * Vector x = {x, vx , ax, ay} |
263 | + * |
264 | + * @param points |
265 | + * @param currentIndex index of the point the filter is currently looking at |
266 | + * @return the input vector X |
267 | + */ |
268 | + private static Matrix createXx(Point current) |
269 | + { |
270 | + Matrix x = new Matrix(2, 1); |
271 | + |
272 | + x.set(0, 0, current.x); |
273 | + x.set(1, 0, current.vx); |
274 | + |
275 | + return x; |
276 | + } |
277 | + |
278 | + private static Matrix createXy(Point cur) |
279 | + { |
280 | + Matrix y = new Matrix(2, 1); |
281 | + |
282 | + y.set(0, 0, cur.y); |
283 | + y.set(1, 0, cur.vy); |
284 | + |
285 | + return y; |
286 | + } |
287 | + /** |
288 | + * Compute the instant velocity and acceleration of each point |
289 | + * <b>This method makes the following assumptions:</b> |
290 | + * 1. Acceleration of a Point object is constant and was properly set in the constructor. |
291 | + * 2. The velocity of the first point is either 0 or has already been correctly calculated. |
292 | + * |
293 | + * TODO: take variable acceleration into account. |
294 | + * |
295 | + * @param points |
296 | + * @return list of point with correct v and a |
297 | + */ |
298 | + private static List<Point> computeVAndA(List<Point> points) |
299 | + { |
300 | + int size = points.size(); |
301 | + double vx_0 = points.get(0).vx; |
302 | + double vy_0 = points.get(0).vy; |
303 | + double ax_0 = points.get(0).ax; |
304 | + double ay_0 = points.get(0).ay; |
305 | + |
306 | + List<Point> result = new ArrayList<Point> (size); |
307 | + result.add(points.get(0)); |
308 | + |
309 | + for (int i = 1; i < size; ++i) |
310 | + { |
311 | + double dt = points.get(i).t - points.get(0).t; |
312 | + double vx = vx_0 + ax_0 * dt; |
313 | + double vy = vy_0 + ay_0 * dt; |
314 | + |
315 | + result.add(new Point(points.get(i).x, |
316 | + points.get(i).y, |
317 | + dt, |
318 | + vx, |
319 | + vy, |
320 | + points.get(i).ax, |
321 | + points.get(i).ay |
322 | + )); |
323 | + } |
324 | + |
325 | + return result; |
326 | + } |
327 | + |
328 | + /** |
329 | + * TODO: |
330 | + * Same as createX(...)'s |
331 | + * |
332 | + * @param inputs |
333 | + * @param currentIndex |
334 | + * @return the state matrix A based on the the current and past position |
335 | + */ |
336 | + private static Matrix createA(Point current) |
337 | + { |
338 | + Matrix A = Matrix.identity(2, 2); |
339 | + A.set(0, 1, current.t); |
340 | + return A; |
341 | + } |
342 | + |
343 | + /** |
344 | + * |
345 | + * @param error the sensor's error |
346 | + * @return the measurement noise covariance matrix |
347 | + */ |
348 | + private static Matrix createR (double error) |
349 | + { |
350 | + Matrix R = new Matrix(1,1); |
351 | + R.set(0, 0, error); |
352 | + return R; |
353 | + } |
354 | + /** |
355 | + * |
356 | + * @return the initial P matrix, which contains all zeroes |
357 | + */ |
358 | + private static Matrix getP0() |
359 | + { |
360 | + double m2x2[][] = new double[VARS_COUNT][VARS_COUNT]; |
361 | + |
362 | + for (int n = 0; n < VARS_COUNT; ++n) |
363 | + Arrays.fill(m2x2[n], 0); |
364 | + |
365 | + return Matrix.constructWithCopy(m2x2); |
366 | + } |
367 | + |
368 | + /** |
369 | + * TODO: |
370 | + * correctly determine this |
371 | + * |
372 | + * @return the process noise covariance matrix |
373 | + */ |
374 | + private static Matrix getQ(Matrix G, double sd) |
375 | + { |
376 | + return G.times(G.transpose()).times(sd * sd); |
377 | + } |
378 | + |
379 | + /** |
380 | + * |
381 | + * @return the [1 by n] observation matrix: [1 0 0 0 .... 0 ] |
382 | + */ |
383 | + private static Matrix getH() |
384 | + { |
385 | + Matrix H = new Matrix(1,2); |
386 | + H.set(0, 0, 1); |
387 | + return H; |
388 | + } |
389 | + |
390 | + private static Matrix createG(Point p) |
391 | + { |
392 | + Matrix ret = new Matrix(2, 1); |
393 | + ret.set(0, 0, p.t * p.t / 2); |
394 | + ret.set(1, 0, p.t); |
395 | + |
396 | + return ret; |
397 | + } |
398 | + |
399 | + private static final String IDEAL = "C:\\Users\\NhatPhan\\Desktop\\Choe\\Sensor Research\\data\\ideal.txt"; |
400 | + private static final String SENSOR = "C:\\Users\\NhatPhan\\Desktop\\Choe\\Sensor Research\\data\\sensor.txt"; |
401 | + private static final String CORRECTED = "C:\\Users\\NhatPhan\\Desktop\\Choe\\Sensor Research\\data\\corrected.txt"; |
402 | + |
403 | + private static final double DEFAULT_NOISE = 0.5; |
404 | + private static final int VARS_COUNT = 2; |
405 | +} |
406 | |
407 | === modified file 'src/kalmanfilter/impl/KalmanFilter.java' |
408 | --- src/kalmanfilter/impl/KalmanFilter.java 2012-07-03 18:46:12 +0000 |
409 | +++ src/kalmanfilter/impl/KalmanFilter.java 2012-07-07 07:30:27 +0000 |
410 | @@ -1,85 +1,33 @@ |
411 | -/** |
412 | - * Implementation of the Kalman Filter |
413 | - * Copyright (C) 2012 Vy Nguyen |
414 | - * |
415 | - * This library is free software; you can redistribute it and/or |
416 | - * modify it under the terms of the GNU Library General Public |
417 | - * License as published by the Free Software Foundation; either |
418 | - * version 2 of the License, or (at your option) any later version. |
419 | - * |
420 | - * This library is distributed in the hope that it will be useful, |
421 | - * but WITHOUT ANY WARRANTY; without even the implied warranty of |
422 | - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
423 | - * Library General Public License for more details. |
424 | - * |
425 | - * You should have received a copy of the GNU Library General Public |
426 | - * License along with this library; if not, write to the |
427 | - * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor, |
428 | - * Boston, MA 02110-1301, USA. |
429 | - */ |
430 | -package kalmanfilter.impl; |
431 | - |
432 | -import Jama.Matrix; |
433 | - |
434 | -/** |
435 | - * Implements the Kalman Filter using Jama's Matrix API |
436 | - * |
437 | - * @author Vy Thao Nguyen |
438 | - */ |
439 | -public class KalmanFilter |
440 | -{ |
441 | - // variable states |
442 | - private Matrix X; // input vector |
443 | - private Matrix P; // state covariance matrix |
444 | - private Matrix Q; // process noise covariance matrix |
445 | - |
446 | - // const states |
447 | - private final Matrix H; // observation matrix |
448 | - private final Matrix I; // the identity matrix |
449 | - private final Matrix A; // state matrix |
450 | - private final Matrix R; // measurement noise covariance matrix |
451 | - private final Matrix B; // control input matrix |
452 | - private final Matrix u; |
453 | - |
454 | - public KalmanFilter(Matrix X, Matrix P, Matrix Q, Matrix H, Matrix A, Matrix R, Matrix B, Matrix u) |
455 | - { |
456 | - this.X = X; |
457 | - this.P = P; |
458 | - this.Q = Q; |
459 | - this.H = H; |
460 | - this.A = A; |
461 | - this.R = R; |
462 | - this.B = B; |
463 | - this.u = u; |
464 | - |
465 | - I = Matrix.identity(P.getRowDimension(), P.getColumnDimension()); |
466 | - } |
467 | - |
468 | - public KalmanFilter timeUpdate() |
469 | - { |
470 | - X = A.times(X).plus(B.times(u)); |
471 | - P = A.times(P).times(A.transpose()).plus(Q); |
472 | - return this; // for easy chaining |
473 | - } |
474 | - |
475 | - /** |
476 | - * |
477 | - * @param Xk: the new measurement (sensor reading) |
478 | - * @return |
479 | - */ |
480 | - public Matrix messurementUpdate(Matrix Xk) |
481 | - { |
482 | - // compute kalman gain |
483 | - //Kk+1 = PkHT(H PkHT + R)-1 |
484 | - Matrix K = P.times(H.transpose()).times((H.times(P).times(H.trace()).plus(R)).inverse()); |
485 | - |
486 | - // update x |
487 | - // xk+1 = xk + K(zk+1 − Hxk ) |
488 | - X = X.plus(K.times((Xk.times(H)).minus(H.times(X)))); |
489 | - |
490 | - // update P |
491 | - P = (I.minus(K.times(H))).times(P); |
492 | - |
493 | - return X; |
494 | - } |
495 | -} |
496 | +/** |
497 | + * Implementation of the Kalman Filter |
498 | + * Copyright (C) 2012 Vy Nguyen |
499 | + * |
500 | + * This library is free software; you can redistribute it and/or |
501 | + * modify it under the terms of the GNU Library General Public |
502 | + * License as published by the Free Software Foundation; either |
503 | + * version 2 of the License, or (at your option) any later version. |
504 | + * |
505 | + * This library is distributed in the hope that it will be useful, |
506 | + * but WITHOUT ANY WARRANTY; without even the implied warranty of |
507 | + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
508 | + * Library General Public License for more details. |
509 | + * |
510 | + * You should have received a copy of the GNU Library General Public |
511 | + * License along with this library; if not, write to the |
512 | + * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor, |
513 | + * Boston, MA 02110-1301, USA. |
514 | + */ |
515 | + |
516 | +package kalmanfilter.impl; |
517 | + |
518 | +import Jama.Matrix; |
519 | + |
520 | +/** |
521 | + * |
522 | + * @author Vy Thao Nguyen |
523 | + */ |
524 | +public interface KalmanFilter |
525 | +{ |
526 | + public KalmanFilter timeUpdate(Matrix A, Matrix Q, Matrix Ga); |
527 | + public Matrix messurementUpdate(Matrix Xk); |
528 | +} |
529 | |
530 | === added file 'src/kalmanfilter/impl/KalmanFilter1.java' |
531 | --- src/kalmanfilter/impl/KalmanFilter1.java 1970-01-01 00:00:00 +0000 |
532 | +++ src/kalmanfilter/impl/KalmanFilter1.java 2012-07-07 07:30:27 +0000 |
533 | @@ -0,0 +1,84 @@ |
534 | +/** |
535 | + * Implementation of the Kalman Filter |
536 | + * Copyright (C) 2012 Vy Nguyen |
537 | + * |
538 | + * This library is free software; you can redistribute it and/or |
539 | + * modify it under the terms of the GNU Library General Public |
540 | + * License as published by the Free Software Foundation; either |
541 | + * version 2 of the License, or (at your option) any later version. |
542 | + * |
543 | + * This library is distributed in the hope that it will be useful, |
544 | + * but WITHOUT ANY WARRANTY; without even the implied warranty of |
545 | + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
546 | + * Library General Public License for more details. |
547 | + * |
548 | + * You should have received a copy of the GNU Library General Public |
549 | + * License along with this library; if not, write to the |
550 | + * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor, |
551 | + * Boston, MA 02110-1301, USA. |
552 | + */ |
553 | +package kalmanfilter.impl; |
554 | + |
555 | +import Jama.Matrix; |
556 | + |
557 | +/** |
558 | + * Implements the original Kalman Filter using Jama's Matrix API |
559 | + * |
560 | + * *original* being all states are encoded into one state matrix. |
561 | + * If you have difficulty encoding all states into one, you might want to consider |
562 | + * using KalmanFilter2.java |
563 | + * |
564 | + * @author Vy Thao Nguyen |
565 | + */ |
566 | +public class KalmanFilter1 implements KalmanFilter |
567 | +{ |
568 | + // variable states |
569 | + private Matrix X; // input vector |
570 | + private Matrix P; // state covariance matrix |
571 | + |
572 | + // const states |
573 | + private final Matrix H; // observation matrix |
574 | + private final Matrix I; // the identity matrix |
575 | + private final Matrix R; // measurement noise covariance matrix |
576 | + |
577 | + public KalmanFilter1(Matrix X, Matrix P, Matrix H, Matrix R) |
578 | + { |
579 | + this.X = X; |
580 | + this.P = P; |
581 | + this.H = H; |
582 | + this.R = R; |
583 | + |
584 | + I = Matrix.identity(P.getRowDimension(), P.getColumnDimension()); |
585 | + } |
586 | + |
587 | + @Override |
588 | + public KalmanFilter1 timeUpdate(Matrix A, Matrix Q, Matrix Ga) |
589 | + { |
590 | + // Matrix Ax = A.times(X); |
591 | + X = (A.times(X)).plus(Ga); |
592 | + P = A.times(P).times(A.transpose()).plus(Q); |
593 | + return this; // for easy chaining |
594 | + } |
595 | + |
596 | + /** |
597 | + * |
598 | + * @param Xk: the new measurement (sensor reading) |
599 | + * @return |
600 | + */ |
601 | + @Override |
602 | + public Matrix messurementUpdate(Matrix Xk) |
603 | + { |
604 | + // compute kalman gain |
605 | + //Kk+1 = PkHT(H PkHT + R)-1 |
606 | + Matrix K = (P.times(H.transpose())).times((H.times(P).times(H.transpose()).plus(R)).inverse()); |
607 | + |
608 | + // update x |
609 | + // xk+1 = xk + K(zk+1 − Hxk ) |
610 | + X = X.plus(K.times((H.times(Xk)).minus(H.times(X)))); |
611 | + |
612 | + // update P |
613 | + P = (I.minus(K.times(H))).times(P); |
614 | + |
615 | + return X; |
616 | + } |
617 | +} |
618 | |
619 | === added file 'src/kalmanfilter/impl/KalmanFilter2.java' |
620 | --- src/kalmanfilter/impl/KalmanFilter2.java 1970-01-01 00:00:00 +0000 |
621 | +++ src/kalmanfilter/impl/KalmanFilter2.java 2012-07-07 07:30:27 +0000 |
622 | @@ -0,0 +1,101 @@ |
623 | +/** |
624 | + * Implementation of the Kalman Filter |
625 | + * Copyright (C) 2012 Vy Nguyen |
626 | + * |
627 | + * This library is free software; you can redistribute it and/or |
628 | + * modify it under the terms of the GNU Library General Public |
629 | + * License as published by the Free Software Foundation; either |
630 | + * version 2 of the License, or (at your option) any later version. |
631 | + * |
632 | + * This library is distributed in the hope that it will be useful, |
633 | + * but WITHOUT ANY WARRANTY; without even the implied warranty of |
634 | + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
635 | + * Library General Public License for more details. |
636 | + * |
637 | + * You should have received a copy of the GNU Library General Public |
638 | + * License along with this library; if not, write to the |
639 | + * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor, |
640 | + * Boston, MA 02110-1301, USA. |
641 | + */ |
642 | + |
643 | +package kalmanfilter.impl; |
644 | + |
645 | +import Jama.Matrix; |
646 | + |
647 | +/** |
648 | + * Implements the filter with different 'groups' of states encoded in different |
649 | + * matrices. |
650 | + * |
651 | + * For eg., |
652 | + * For a system with the following states: |
653 | + * x: horizontal position |
654 | + * y: vertical position |
655 | + * u: horizontal velocity |
656 | + * v: vertical velocity |
657 | + * |
658 | + * divided into two groups: X1 = {x, u}, X2 = {y, v} |
659 | + * |
660 | + * TODO: make the number of 'groups' variable. Currently, this is only |
661 | + * capable of handling a fixed 2 groups |
662 | + * |
663 | + * @author Vy Thao Nguyen |
664 | + */ |
665 | +public class KalmanFilter2 //implements KalmanFilter |
666 | +{ |
667 | + // variable states |
668 | + private Matrix X; // input vector |
669 | + private Matrix P; // state covariance matrix |
670 | + private Matrix Q; // process noise covariance matrix |
671 | + |
672 | + // const states |
673 | + private final Matrix H; // observation matrix |
674 | + private final Matrix I; // the identity matrix |
675 | + private final Matrix A; // state matrix |
676 | + private final Matrix R; // measurement noise covariance matrix |
677 | + private final Matrix B; // control input matrix |
678 | + private final Matrix u; |
679 | + |
680 | + public KalmanFilter2(Matrix X, Matrix P, Matrix Q, Matrix H, Matrix A, Matrix R, Matrix B, Matrix u) |
681 | + { |
682 | + this.X = X; |
683 | + this.P = P; |
684 | + this.Q = Q; |
685 | + this.H = H; |
686 | + this.A = A; |
687 | + this.R = R; |
688 | + this.B = B; |
689 | + this.u = u; |
690 | + |
691 | + I = Matrix.identity(P.getRowDimension(), P.getColumnDimension()); |
692 | + } |
693 | + |
694 | + // @Override |
695 | + public KalmanFilter2 timeUpdate() |
696 | + { |
697 | + X = A.times(X).plus(B.times(u)); |
698 | + P = A.times(P).times(A.transpose()).plus(Q); |
699 | + return this; // for easy chaining |
700 | + } |
701 | + |
702 | + /** |
703 | + * |
704 | + * @param Xk: the new measurement (sensor reading) |
705 | + * @return |
706 | + */ |
707 | + // @Override |
708 | + public Matrix messurementUpdate(Matrix Xk) |
709 | + { |
710 | + // compute kalman gain |
711 | + //Kk+1 = PkHT(H PkHT + R)-1 |
712 | + Matrix K = P.times(H.transpose()).times((H.times(P).times(H.trace()).plus(R)).inverse()); |
713 | + |
714 | + // update x |
715 | + // xk+1 = xk + K(zk+1 − Hxk ) |
716 | + X = X.plus(K.times((Xk.times(H)).minus(H.times(X)))); |
717 | + |
718 | + // update P |
719 | + P = (I.minus(K.times(H))).times(P); |
720 | + |
721 | + return X; |
722 | + } |
723 | +} |