Wording changes.

07-Kalman-Filter-Math.ipynb

@@ -785,15 +785,17 @@
     "\n",
     "$$\\mathbf Q = \\int_0^{\\Delta t} \\mathbf F(t)\\mathbf{Q_c}\\mathbf F^\\mathsf{T}(t) dt$$\n",
     "\n",
-    "where $\\mathbf{Q_c}$ is the continuous noise. This gives us\n",
+    "where $\\mathbf{Q_c}$ is the continuous noise. The general reasoning should be clear. $\\mathbf F(t)\\mathbf{Q_c}\\mathbf F^\\mathsf{T}(t)$ is a projection of the continuous noise based on our process model $\\mathbf F(t)$ at the instant $t$. We want to know how much noise is added to the system over a discrete intervat $\\Delta t$, so we integrate this expression over the interval $[0, \\Delta t]$. \n",
     "\n",
-    "$$\\Phi = \\begin{bmatrix}1 & \\Delta t & {\\Delta t}^2/2 \\\\ 0 & 1 & \\Delta t\\\\ 0& 0& 1\\end{bmatrix}$$\n",
+    "We know the fundamental matrix for Newtonian systems is\n",
     "\n",
-    "for the fundamental matrix, and\n",
+    "$$F = \\begin{bmatrix}1 & \\Delta t & {\\Delta t}^2/2 \\\\ 0 & 1 & \\Delta t\\\\ 0& 0& 1\\end{bmatrix}$$\n",
+    "\n",
+    "We define the continuous noise as \n",
     "\n",
     "$$\\mathbf{Q_c} = \\begin{bmatrix}0&0&0\\\\0&0&0\\\\0&0&1\\end{bmatrix} \\Phi_s$$\n",
     "\n",
-    "for the continuous process noise matrix, where $\\Phi_s$ is the spectral density of the white noise.\n",
+    "where $\\Phi_s$ is the spectral density of the white noise. This can be derived, but is beyond the scope of this book. See any standard text on stoochastic processes for the details. In practice we often do not know the spectral density of the noise, and so this turns into an \"engineering\" factor - a number we experimentally tune until our filter performs as we expect. You can see that the matrix that $\\Phi_s$ is multiplied by effectively assigns the power spectral density to the acceleration term. This makes sense; we assume that the system has constant acceleration except for the variations caused by noise. The noise alters the acceleration.\n",
     "\n",
     "We could carry out these computations ourselves, but I prefer using SymPy to solve the equation.\n",
     "\n",
@@ -1250,7 +1252,7 @@
     "\n",
     "$$\\mathbf Q=\\begin{bmatrix}0&0&0\\\\0&0&0\\\\0&0&\\sigma^2\\end{bmatrix}$$\n",
     "\n",
-    "while not correct, is often a useful approximation. If you do this you will have to perform quite a few studies to guarantee that your filter works in a variety of situations. \n",
+    "while not correct, is often a useful approximation. If you do this for an important application you will have to perform quite a few studies to guarantee that your filter works in a variety of situations. \n",
     "\n",
     "If you do this, 'lower right term' means the most rapidly changing term for each variable. If the state is $x=\\begin{bmatrix}x & \\dot x & \\ddot{x} & y & \\dot{y} & \\ddot{y}\\end{bmatrix}^\\mathsf{T}$ Then Q will be 6x6; the elements for both $\\ddot{x}$ and $\\ddot{y}$ will have to be set to non-zero in $\\mathbf Q$."
    ]
@@ -1756,7 +1758,7 @@
  "metadata": {
   "anaconda-cloud": {},
   "kernelspec": {
-   "display_name": "Python [default]",
+   "display_name": "Python 3",
    "language": "python",
    "name": "python3"
   },
@@ -1770,7 +1772,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.5.1"
+   "version": "3.5.2"
   }
  },
  "nbformat": 4,

10-Unscented-Kalman-Filter.ipynb

@@ -3451,7 +3451,7 @@
  "metadata": {
   "anaconda-cloud": {},
   "kernelspec": {
-   "display_name": "Python [default]",
+   "display_name": "Python 3",
    "language": "python",
    "name": "python3"
   },
@@ -3465,7 +3465,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.5.1"
+   "version": "3.5.2"
   }
  },
  "nbformat": 4,

11-Extended-Kalman-Filters.ipynb

@@ -1869,7 +1869,7 @@
  "metadata": {
   "anaconda-cloud": {},
   "kernelspec": {
-   "display_name": "Python [default]",
+   "display_name": "Python 3",
    "language": "python",
    "name": "python3"
   },
@@ -1883,7 +1883,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.5.1"
+   "version": "3.5.2"
   }
  },
  "nbformat": 4,

Appendix-E-Ensemble-Kalman-Filters.ipynb

@@ -767,7 +767,7 @@
  "metadata": {
   "anaconda-cloud": {},
   "kernelspec": {
-   "display_name": "Python [default]",
+   "display_name": "Python 3",
    "language": "python",
    "name": "python3"
   },
@@ -781,7 +781,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.5.1"
+   "version": "3.5.2"
   }
  },
  "nbformat": 4,

code/mkf_internal.py

@@ -374,7 +374,7 @@ def plot_3_covariances():
     plt.gca().grid(b=False)
     plt.gca().set_xticks([0,1,2,3,4])
     plot_covariance_ellipse((2, 7), cov=P, facecolor='g', alpha=0.2,
-                            title='|2 0|\n|0 2|', std=[1,2,3], axis_equal=False)
+                            title='|2 0|\n|0 2|', std=[3], axis_equal=False)
     plt.ylim((0, 15))
     plt.gca().set_aspect('equal', adjustable='box')
 
@@ -385,7 +385,7 @@ def plot_3_covariances():
     plt.ylim((0, 15))
     plt.gca().set_aspect('equal', adjustable='box')
     plot_covariance_ellipse((2, 7), P, facecolor='g', alpha=0.2,
-                            std=[1,2,3],axis_equal=False, title='|2 0|\n|0 6|')
+                            std=[3],axis_equal=False, title='|2 0|\n|0 6|')
 
     plt.subplot(133)
     plt.gca().grid(b=False)
@@ -394,8 +394,8 @@ def plot_3_covariances():
     plt.ylim((0, 15))
     plt.gca().set_aspect('equal', adjustable='box')
     plot_covariance_ellipse((2, 7), P, facecolor='g', alpha=0.2,
-                            axis_equal=False,std=[1,2,3],
-                            title='|2 1.2|\n|1.2 2|')
+                            axis_equal=False,std=[3],
+                            title='|2.0 1.2|\n|1.2 2.0|')
 
     plt.tight_layout()
     plt.show()
@@ -450,15 +450,16 @@ def plot_track(ps, actual, zs, cov, std_scale=1,
 
         plt.xlim((0,count))
         plt.title(title)
+        plt.show()
 
     if plot_P:
-        with interactive_plot():
-            ax = plt.subplot(121)
-            ax.set_title("$\sigma^2_x$ (pos variance)")
-            plot_covariance(cov, (0, 0))
-            ax = plt.subplot(122)
-            ax.set_title("$\sigma^2_\dot{x}$ (vel variance)")
-            plot_covariance(cov, (1, 1))
+        ax = plt.subplot(121)
+        ax.set_title("$\sigma^2_x$ (pos variance)")
+        plot_covariance(cov, (0, 0))
+        ax = plt.subplot(122)
+        ax.set_title("$\sigma^2_\dot{x}$ (vel variance)")
+        plot_covariance(cov, (1, 1))
+        plt.show()
 
 
 def plot_covariance(P, index=(0, 0)):

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)