Wording changes.

experiments/slam.py

@@ -0,0 +1,179 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Mon Oct  3 07:59:40 2016
+
+@author: rlabbe
+"""
+
+from filterpy.kalman import UnscentedKalmanFilter as UKF, MerweScaledSigmaPoints
+from math import tan, sin, cos, sqrt, atan2
+import numpy as np
+from numpy.random import randn
+import matplotlib.pyplot as plt
+from filterpy.stats import plot_covariance_ellipse
+
+
+
+def move(x, u, dt, wheelbase):
+    hdg = x[2]
+    vel = u[0]
+    steering_angle = u[1]
+    dist = vel * dt
+
+    if abs(steering_angle) > 0.001: # is robot turning?
+        beta = (dist / wheelbase) * tan(steering_angle)
+        r = wheelbase / tan(steering_angle) # radius
+
+        sinh, sinhb = sin(hdg), sin(hdg + beta)
+        cosh, coshb = cos(hdg), cos(hdg + beta)
+        return x + np.array([-r*sinh + r*sinhb,
+                              r*cosh - r*coshb, beta])
+    else: # moving in straight line
+        return x + np.array([dist*cos(hdg), dist*sin(hdg), 0])
+
+
+def fx(x, dt, u):
+    return move(x, u, dt, wheelbase)
+
+
+def normalize_angle(x):
+    x = x % (2 * np.pi)    # force in range [0, 2 pi)
+    if x > np.pi:          # move to [-pi, pi)
+        x -= 2 * np.pi
+    return x
+
+
+def residual_h(a, b):
+    y = a - b
+    # data in format [dist_1, bearing_1, dist_2, bearing_2,...]
+    for i in range(0, len(y), 2):
+        y[i + 1] = normalize_angle(y[i + 1])
+    return y
+
+
+def residual_x(a, b):
+    y = a - b
+    y[2] = normalize_angle(y[2])
+    return y
+
+
+def aa(x, y):
+    if y is not None:
+        return x % y
+    else:
+        return x
+
+def bb(x,y):
+    try:
+        return x % y
+    except:
+        return x
+
+
+
+def Hx(x, landmarks):
+    """ takes a state variable and returns the measurement
+    that would correspond to that state. """
+    hx = []
+    for lmark in landmarks:
+        px, py = lmark
+        dist = sqrt((px - x[0])**2 + (py - x[1])**2)
+        angle = atan2(py - x[1], px - x[0])
+        hx.extend([dist, normalize_angle(angle - x[2])])
+    return np.array(hx)
+
+
+def state_mean(sigmas, Wm):
+    x = np.zeros(3)
+
+    sum_sin = np.sum(np.dot(np.sin(sigmas[:, 2]), Wm))
+    sum_cos = np.sum(np.dot(np.cos(sigmas[:, 2]), Wm))
+    x[0] = np.sum(np.dot(sigmas[:, 0], Wm))
+    x[1] = np.sum(np.dot(sigmas[:, 1], Wm))
+    x[2] = atan2(sum_sin, sum_cos)
+    return x
+
+
+def z_mean(sigmas, Wm):
+    z_count = sigmas.shape[1]
+    x = np.zeros(z_count)
+
+    for z in range(0, z_count, 2):
+        sum_sin = np.sum(np.dot(np.sin(sigmas[:, z+1]), Wm))
+        sum_cos = np.sum(np.dot(np.cos(sigmas[:, z+1]), Wm))
+
+        x[z] = np.sum(np.dot(sigmas[:,z], Wm))
+        x[z+1] = atan2(sum_sin, sum_cos)
+    return x
+
+
+dt = 1.0
+wheelbase = 0.5
+
+def run_localization(
+    cmds, landmarks, sigma_vel, sigma_steer, sigma_range,
+    sigma_bearing, ellipse_step=1, step=10):
+
+    plt.figure()
+    points = MerweScaledSigmaPoints(n=3, alpha=.00001, beta=2, kappa=0,
+                                    subtract=residual_x)
+    ukf = UKF(dim_x=3, dim_z=2*len(landmarks), fx=fx, hx=Hx,
+              dt=dt, points=points, x_mean_fn=state_mean,
+              z_mean_fn=z_mean, residual_x=residual_x,
+              residual_z=residual_h)
+
+    ukf.x = np.array([2, 6, .3])
+    ukf.P = np.diag([.1, .1, .05])
+    ukf.R = np.diag([sigma_range**2,
+                     sigma_bearing**2]*len(landmarks))
+    ukf.Q = np.eye(3)*0.0001
+
+    sim_pos = ukf.x.copy()
+
+    # plot landmarks
+    if len(landmarks) > 0:
+        plt.scatter(landmarks[:, 0], landmarks[:, 1],
+                    marker='s', s=60)
+
+    track = []
+    for i, u in enumerate(cmds):
+        sim_pos = move(sim_pos, u, dt/step, wheelbase)
+        track.append(sim_pos)
+
+        if i % step == 0:
+            ukf.predict(fx_args=u)
+
+            if i % ellipse_step == 0:
+                plot_covariance_ellipse(
+                    (ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6,
+                     facecolor='k', alpha=0.3)
+
+            x, y = sim_pos[0], sim_pos[1]
+            z = []
+            for lmark in landmarks:
+                dx, dy = lmark[0] - x, lmark[1] - y
+                d = sqrt(dx**2 + dy**2) + randn()*sigma_range
+                bearing = atan2(lmark[1] - y, lmark[0] - x)
+                a = (normalize_angle(bearing - sim_pos[2] +
+                     randn()*sigma_bearing))
+                z.extend([d, a])
+            ukf.update(z, hx_args=(landmarks,))
+
+            if i % ellipse_step == 0:
+                plot_covariance_ellipse(
+                    (ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6,
+                     facecolor='g', alpha=0.8)
+    track = np.array(track)
+    plt.plot(track[:, 0], track[:,1], color='k', lw=2)
+    plt.axis('equal')
+    plt.title("UKF Robot localization")
+    plt.show()
+    return ukf
+
+
+landmarks = np.array([[5, 10], [10, 5], [15, 15]])
+cmds = [np.array([1.1, .01])] * 200
+ukf = run_localization(
+    cmds, landmarks, sigma_vel=0.1, sigma_steer=np.radians(1),
+    sigma_range=0.3, sigma_bearing=0.1)
+print('Final P:', ukf.P.diagonal())

experiments/slamekf.py

@@ -0,0 +1,125 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Sun Oct  9 08:25:01 2016
+
+@author: roger
+"""
+
+import numpy as np
+
+
+class WorldMap(object):
+    
+    def __init__(self, N=100):
+        
+        self.N = N
+        pass
+        
+        
+        
+        
+    def measurements(self, x, theta):
+        """ return array of measurements (range, angle) if robot is in position
+        x"""
+    
+        N = 10
+        a = np.linspace(-np.pi, np.pi, self.N)
+        return a
+        
+        
+
+def get_line(start, end):
+    """Bresenham's Line Algorithm
+    Produces a list of tuples from start and end
+ 
+    >>> points1 = get_line((0, 0), (3, 4))
+    >>> points2 = get_line((3, 4), (0, 0))
+    >>> assert(set(points1) == set(points2))
+    >>> print points1
+    [(0, 0), (1, 1), (1, 2), (2, 3), (3, 4)]
+    >>> print points2
+    [(3, 4), (2, 3), (1, 2), (1, 1), (0, 0)]
+    
+    source:
+    http://www.roguebasin.com/index.php?title=Bresenham%27s_Line_Algorithm
+    """
+    # Setup initial conditions
+    x1, y1 = int(round(start[0])), int(round(start[1]))
+    x2, y2 = int(round(end[0])), int(round(end[1]))
+    dx = x2 - x1
+    dy = y2 - y1
+ 
+    # Determine how steep the line is
+    is_steep = abs(dy) > abs(dx)
+ 
+    # Rotate line
+    if is_steep:
+        x1, y1 = y1, x1
+        x2, y2 = y2, x2
+ 
+    # Swap start and end points if necessary and store swap state
+    swapped = False
+    if x1 > x2:
+        x1, x2 = x2, x1
+        y1, y2 = y2, y1
+        swapped = True
+    # Recalculate differentials
+    dx = x2 - x1
+    dy = y2 - y1
+ 
+    # Calculate error
+    error = int(dx / 2.0)
+    ystep = 1 if y1 < y2 else -1
+ 
+    # Iterate over bounding box generating points between start and end
+    y = y1
+    points = []
+    for x in range(x1, x2 + 1):
+        coord = (y, x) if is_steep else (x, y)
+        points.append(coord)
+        error -= abs(dy)
+        if error < 0:
+            y += ystep
+            error += dx
+ 
+    # Reverse the list if the coordinates were swapped
+    if swapped:
+        points.reverse()
+    return points 
+ 
+
+world = np.zeros((1000,1000), dtype=bool)
+
+
+def add_line(p0, p1):
+    pts = get_line(p0, p1)
+    for p in pts:
+        try:
+            world[p[0], p[1]] = True
+        except:
+            pass # ignore out of range
+        
+        
+add_line((0,0), (1000, 0))
+
+def measure(x, theta):
+
+    dx,dy = world.shape
+    h = np.sqrt(2*(dx*dx + dy+dy))
+    p1 = [h*np.cos(theta), h*np.sin(theta)]
+
+    
+    hits = get_line(x, p1)
+    
+    try:
+        for pt in hits:
+            if world[pt[0], pt[1]]:
+                return pt
+    except:
+        return -1
+    return -2
+    
+    
+    
+
+measure([100,100], -np.pi/2)