Merge lp:~vytng/kalmanfilterimpl/supprot_B_u into lp:kalmanfilterimpl

[Vy Thuy Nguyen]

- support B and u control signals
- change RunFilter accordingly

[Vy Nguyen (oontvoo)]

Looks good!

review: Approve

=== modified file 'nbproject/private/private.properties'
--- nbproject/private/private.properties	2012-06-26 08:40:46 +0000
+++ nbproject/private/private.properties	2012-06-28 02:32:21 +0000
@@ -1,2 +1,2 @@
 compile.on.save=true
-user.properties.file=C:\\Users\\NhatPhan\\.netbeans\\7.1.2\\build.properties
+user.properties.file=C:\\Users\\VyNguyen\\.netbeans\\7.1\\build.properties

=== modified file 'src/kalmanfilter/KalmanFilter.java'
--- src/kalmanfilter/KalmanFilter.java	2012-06-27 01:47:04 +0000
+++ src/kalmanfilter/KalmanFilter.java	2012-06-28 02:32:21 +0000
@@ -23,7 +23,6 @@
 
 /**
  * Implements the Kalman Filter using Jama's Matrix API
- * For simplicity, the control input (signal) is assumed to be 0
  * 
  * @author Vy Thao Nguyen
  */
@@ -39,8 +38,10 @@
     private final Matrix I; // the identity matrix 
     private final Matrix A; // state matrix
     private final Matrix R; // measurement noise covariance matrix
+    private final Matrix B; // control input matrix
+    private final Matrix u; 
     
-    public KalmanFilter(Matrix X, Matrix P, Matrix Q, Matrix H, Matrix A, Matrix R)
+    public KalmanFilter(Matrix X, Matrix P, Matrix Q, Matrix H, Matrix A, Matrix R, Matrix B, Matrix u)
     {
         this.X = X;
         this.P = P;
@@ -48,12 +49,15 @@
         this.H = H;
         this.A = A;
         this.R = R;
+        this.B = B;
+        this.u = u;
+        
         I = Matrix.identity(P.getRowDimension(), P.getColumnDimension());
     }
 
     public KalmanFilter timeUpdate()
     {
-        X = A.times(X);
+        X = A.times(X).plus(B.times(u));
         P = A.times(P).times(A.transpose()).plus(Q);
         return this; // for easy chaining
     }

=== modified file 'src/kalmanfilter/RunFilter.java'
--- src/kalmanfilter/RunFilter.java	2012-06-28 00:21:09 +0000
+++ src/kalmanfilter/RunFilter.java	2012-06-28 02:32:21 +0000
@@ -63,7 +63,9 @@
                                                getQ(),
                                                getH(),
                                                createA(inputs, 0),
-                                               createR(sensorNoise));
+                                               createR(sensorNoise),
+                                               createB(),
+                                               createU());
              
         for (int n = 1; n < inputs.size(); ++n)
         {
@@ -159,5 +161,26 @@
         H.set(0, 0, 1);
         return H;
     }
+    
+    /**
+     * TODO: determine the value of B
+     * (could take an argument)
+     * @return 
+     */
+    private static Matrix createB()
+    {
+        throw new UnsupportedOperationException();
+    }
+    
+    /**
+     * TODO: similar to B
+     * 
+     * @return 
+     */
+    private static Matrix createU()
+    {
+        throw new UnsupportedOperationException();
+    }
+    
     private static final int VARS_COUNT = 6;
 }