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

[Vy Thuy Nguyen]

- add computeVAndA()
- implement createX()

[lp:~vytng/kalmanfilterimpl/supportVelocity]

6. By vy thuy nguyen(<email address hidden>) on 2012-06-28

Fix thinkos in computeVAndA()

7. By vy thuy nguyen(<email address hidden>) on 2012-06-28

More thinkos

[Vy Nguyen (oontvoo)]

Looks great! Thanks

review: Approve

=== modified file 'src/kalmanfilter/RunFilter.java'
--- src/kalmanfilter/RunFilter.java	2012-06-28 02:28:33 +0000
+++ src/kalmanfilter/RunFilter.java	2012-06-28 04:10:24 +0000
@@ -32,17 +32,40 @@
  */
 public class RunFilter 
 {
+    /**
+     * Represents a point
+     */
     public static class Point implements Comparable<Point>
     {
         final double x; // x coord
         final double y; // y coord
         final double t; // time coord
-        public Point(double x, double y, double t)
+        final double vx; // instant x-velocity
+        final double vy; // instant y-velocity
+        final double ax; // instant x-acceleration
+        final double ay; // instant y-acceleration
+        
+        /**
+         * 
+         * @param x
+         * @param y
+         * @param t
+         * @param vx
+         * @param vy
+         * @param ax
+         * @param ay
+         */
+        public Point(double x, double y, double t, double vx, double vy, double ax, double ay)
         {
             this.x = x;
             this.y = y;
             this.t = t;
+            this.vx = vx;
+            this.vy = vy;
+            this.ax = ax;
+            this.ay = ay;
         }
+        
         @Override
         public int compareTo(Point o)
         {
@@ -50,6 +73,12 @@
         }
     }
     
+    /**
+     * 
+     * @param inputs
+     * @param sensorNoise
+     * @return
+     */
     public static List<Point> filter(List<Point> inputs, double sensorNoise)
     {
         // sort the points in time order
@@ -77,32 +106,85 @@
     }
     
     /**
+     * TODO: correctly decode the state matrix into a point.
+     * (that is, retrieve a and v)
      * 
      * @param X the corrected state matrix
      * @return the corrected value of (x,y,t)
      */
     private static Point decodeX(Matrix X, double t)
     {
-        return new Point(X.get(0, 0), X.get(0, 1), t);
+        //return new Point(X.get(0, 0), X.get(0, 1), t, 0, 0);
+        throw new UnsupportedOperationException();
     }
     
     /**
-     * TODO:
-     * calculate the instance velocity v_x, v_y, a_x and a_y based on
-     * the current and past positions
+     * TODO: correctly encode the point (velocity, acceleration, etc.) into a matrix.
      * 
      * Vector x = {x, y, vx , vy , ax, ay}
      * 
-     * @param inputs set of points 
-     * @param currentIndex
+     * @param points 
+     * @param currentIndex index of the point the filter is currently looking at
      * @return the input vector X
      */
-    private static Matrix createX(List<Point> inputs, int currentIndex)
+    private static Matrix createX(List<Point> points, int currentIndex)
     {
-        throw new UnsupportedOperationException();
+        Matrix x = new Matrix(1, 6);
+        
+        List<Point> updatedPoints = computeVAndA(points);
+        
+        x.set(0, 0, updatedPoints.get(currentIndex).x);
+        x.set(0, 1, updatedPoints.get(currentIndex).y);
+        x.set(0, 2, updatedPoints.get(currentIndex).vx);
+        x.set(0, 3, updatedPoints.get(currentIndex).vy);
+        x.set(0, 4, updatedPoints.get(currentIndex).ax);
+        x.set(0, 5, updatedPoints.get(currentIndex).ay);
+        
+        return x;
     }
 
     /**
+     * Compute the instant velocity and acceleration of each point
+     * <b>This method makes the following assumptions:</b>
+     *      1. Acceleration of a Point object is constant and was properly set in the constructor.
+     *      2. The velocity of the first point is either 0 or has already been correctly calculated.
+     *    
+     * TODO: take variable acceleration into account.
+     * 
+     * @param points
+     * @return list of point with correct v and a 
+     */
+    private static List<Point> computeVAndA(List<Point> points)
+    {
+        int size = points.size();
+        double vx_0 = points.get(0).vx;
+        double vy_0 = points.get(0).vy;
+        double ax_0 = points.get(0).ax;
+        double ay_0 = points.get(0).ay;   
+        
+        List<Point> result = new ArrayList<Point> (size);
+        result.add(points.get(0));
+        
+        for (int i = 1; i < size; ++i)
+        {
+            double dt = points.get(i).t - points.get(0).t;
+            double vx = vx_0 + ax_0 * dt;
+            double vy = vy_0 + ay_0 * dt;
+            
+            result.add(new Point(points.get(i).x,
+                                 points.get(i).y,
+                                 points.get(i).t,
+                                 vx,
+                                 vy,
+                                 points.get(i).ax,
+                                 points.get(i).ay
+                        ));
+        }
+        
+        return result;
+    }
+    
+    /**
      * TODO:
      * Same as createX(...)'s
      *