Added EKF robot localization example.

Still needs a lot of explanation; mostly the implementation is there
for now.

experiments/bicycle.py

@@ -9,6 +9,7 @@ Created on Tue Apr 28 08:19:21 2015
 from math import *
 import numpy as np
 import matplotlib.pyplot as plt
+from matplotlib.patches import Polygon
 
 wheelbase = 100 #inches
 
@@ -21,17 +22,17 @@ pos = np.array([0., 0.]
 
 for i in range(100):
 #if abs(steering_angle) > 1.e-8:
-    dist = vel*t
     turn_radius = tan(steering_angle)
     radius = wheelbase / tan(steering_angle)
 
+    dist = vel*t
     arc_len = dist / (2*pi*radius)
 
     turn_angle = 2*pi * arc_len
 
 
-    cx = pos[0] - (sin(orientation) * radius)
-    cy = pos[1] + (cos(orientation) * radius)
+    cx = pos[0] - radius * sin(orientation)
+    cy = pos[1] + radius * cos(orientation)
 
     orientation = (orientation + turn_angle) % (2.0 * pi)
     pos[0] = cx + (sin(orientation) * radius)
@@ -41,3 +42,6 @@ for i in range(100):
 
 plt.axis('equal')
 
+
+
+