Started writing the Designing Kalman Filter chapter. Did a robot, and DME.

KalmanFilter.py

@@ -14,7 +14,7 @@ class KalmanFilter:
 
     def __init__(self, dim):
         """ Create a Kalman filter of dimension 'dim'"""
-        
+
         self.x = 0 # state
         self.P = np.matrix(np.eye(dim)) # uncertainty covariance
         self.Q = np.matrix(np.eye(dim)) # process uncertainty
@@ -50,12 +50,12 @@ class KalmanFilter:
 if __name__ == "__main__":
     f = KalmanFilter (dim=2)
 
-    f.x = np.matrix([[2.], 
+    f.x = np.matrix([[2.],
                      [0.]])       # initial state (location and velocity)
-                     
+
     f.F = np.matrix([[1.,1.],
                      [0.,1.]])    # state transition matrix
-                     
+
     f.H = np.matrix([[1.,0.]])    # Measurement function
     f.P *= 1000.                  # covariance matrix
     f.R = 5                       # state uncertainty
@@ -66,15 +66,15 @@ if __name__ == "__main__":
     for t in range (100):
         # create measurement = t plus white noise
         z = t + random.randn()*20
-        
+
         # perform kalman filtering
         f.measure (z)
         f.predict()
-        
+
         # save data
         results.append (f.x[0,0])
         measurements.append(z)
-              
+
     # plot data
     p1, = plt.plot(measurements,'r')
     p2, = plt.plot (results,'b')

Untitled0.ipynb

@@ -1,7 +1,7 @@
 {
  "metadata": {
   "name": "",
-  "signature": "sha256:241f0a3c311e0e6a1249928d59085691f69464061378e912134697ff059f8eee"
+  "signature": "sha256:809258e67f36bb99b0ddfd9562b077d11998a453ceb95462f31fbf4d78be5292"
  },
  "nbformat": 3,
  "nbformat_minor": 0,
@@ -43,6 +43,12 @@
       "\n",
       "\n",
       "Here is some text.\n",
+      "\n",
+      "    def this_is_code():\n",
+      "       pass\n",
+      "       </code>\n",
+      "       \n",
+      "       \n",
       "*italic*\n",
       "**bold**\n",
       "\n",

exp/KalmanFilter.py

@@ -0,0 +1,85 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Fri Oct 18 18:02:07 2013
+
+@author: rlabbe
+"""
+
+import numpy as np
+import scipy.linalg as linalg
+import matplotlib.pyplot as plt
+import numpy.random as random
+
+class KalmanFilter:
+
+    def __init__(self, dim):
+        """ Create a Kalman filter of dimension 'dim'"""
+
+        self.x = 0 # state
+        self.P = np.matrix(np.eye(dim)) # uncertainty covariance
+        self.Q = np.matrix(np.eye(dim)) # process uncertainty
+        self.u = np.matrix(np.zeros((dim,1))) # motion vector
+        self.B = 0
+        self.F = 0 # state transition matrix
+        self.H = 0 # Measurement function (maps state to measurements)
+        self.R = np.matrix(np.eye(1)) # state uncertainty
+        self.I = np.matrix(np.eye(dim))
+
+
+    def update(self, Z):
+        """
+        Add a new measurement to the kalman filter.
+        """
+
+        # measurement update
+        y = Z - (self.H * self.x)                   # error (residual) between measurement and prediction
+        S = (self.H * self.P * self.H.T) + self.R   # project system uncertainty into measurment space + measurement noise(R)
+
+
+        K = self.P * self.H.T * linalg.inv(S) # map system uncertainty into kalman gain
+
+        self.x = self.x + (K*y)                # predict new x with residual scaled by the kalman gain
+        self.P = (self.I - (K*self.H))*self.P  # and compute the new covariance
+
+    def predict(self):
+        # prediction
+        self.x = (self.F*self.x) + self.u
+        self.P = self.F * self.P * self.F.T + self.Q
+
+
+if __name__ == "__main__":
+    f = KalmanFilter (dim=2)
+
+    f.x = np.matrix([[2.],
+                     [0.]])       # initial state (location and velocity)
+
+    f.F = np.matrix([[1.,1.],
+                     [0.,1.]])    # state transition matrix
+
+    f.H = np.matrix([[1.,0.]])    # Measurement function
+    f.P *= 1000.                  # covariance matrix
+    f.R = 5                       # state uncertainty
+    f.Q *= 0.0001                 # process uncertainty
+
+    measurements = []
+    results = []
+    for t in range (100):
+        # create measurement = t plus white noise
+        z = t + random.randn()*20
+
+        # perform kalman filtering
+        f.measure (z)
+        f.predict()
+
+        # save data
+        results.append (f.x[0,0])
+        measurements.append(z)
+
+    # plot data
+    p1, = plt.plot(measurements,'r')
+    p2, = plt.plot (results,'b')
+    p3, = plt.plot ([0,100],[0,100], 'g') # perfect result
+    plt.legend([p1,p2, p3], ["noisy measurement", "KF output", "ideal"], 4)
+
+
+    plt.show()

exp/range_finder.py

@@ -0,0 +1,118 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Sat May 24 19:14:06 2014
+
+@author: rlabbe
+"""
+from __future__ import division, print_function
+from KalmanFilter import KalmanFilter
+import numpy as np
+import matplotlib.pyplot as plt
+import numpy.random as random
+import math
+
+
+class DMESensor(object):
+    def __init__(self, pos_a, pos_b, noise_factor=1.0):
+        self.A = pos_a
+        self.B = pos_b
+        self.noise_factor = noise_factor
+
+    def range_of (self, pos):
+        """ returns tuple containing noisy range data to A and B
+        given a position 'pos'
+        """
+
+        ra = math.sqrt((self.A[0] - pos[0])**2 + (self.A[1] - pos[1])**2)
+        rb = math.sqrt((self.B[0] - pos[0])**2 + (self.B[1] - pos[1])**2)
+
+        return (ra + random.randn()*self.noise_factor,
+                rb + random.randn()*self.noise_factor)
+
+
+def dist(a,b):
+    return math.sqrt ((a[0]-b[0])**2 + (a[1]-b[1])**2)
+
+def H_of (pos, pos_A, pos_B):
+    from math import sin, cos, atan2
+
+    theta_a = atan2(pos_a[1]-pos[1], pos_a[0] - pos[0])
+    theta_b = atan2(pos_b[1]-pos[1], pos_b[0] - pos[0])
+
+    return np.mat([[-cos(theta_a), 0, -sin(theta_a), 0],
+                   [-cos(theta_b), 0, -sin(theta_b), 0]])
+
+    # equivalently we can do this...
+    #dist_a = dist(pos, pos_A)
+    #dist_b = dist(pos, pos_B)
+
+    #return np.mat([[(pos[0]-pos_A[0])/dist_a, 0, (pos[1]-pos_A[1])/dist_a,0],
+    #               [(pos[0]-pos_B[0])/dist_b, 0, (pos[1]-pos_B[1])/dist_b,0]])
+
+
+
+
+pos_a = (100,-20)
+pos_b = (-100, -20)
+
+f1 = KalmanFilter(dim=4)
+dt = 1.0   # time step
+'''
+f1.F = np.mat ([[1, dt, 0,  0],
+                [0,  1, 0,  0],
+                [0,  0, 1, dt],
+                [0,  0, 0,  1]])
+
+'''
+f1.F = np.mat ([[0, 1, 0, 0],
+                [0, 0, 0, 0],
+                [0, 0, 0, 1],
+                [0, 0, 0, 0]])
+
+f1.B = 0.
+
+f1.R = np.eye(2) * 1.
+f1.Q = np.eye(4) * .1
+
+f1.x = np.mat([1,0,1,0]).T
+f1.P = np.eye(4) * 5.
+
+# initialize storage and other variables for the run
+count = 30
+xs, ys = [],[]
+pxs, pys = [],[]
+
+d = DMESensor (pos_a, pos_b, noise_factor=1.)
+
+pos = [0,0]
+for i in range(count):
+    pos = (i,i)
+    ra,rb = d.range_of(pos)
+    rx,ry = d.range_of((i+f1.x[0,0], i+f1.x[2,0]))
+
+    print ('range =', ra,rb)
+
+    z = np.mat([[ra-rx],[rb-ry]])
+    print('z =', z)
+
+    f1.H = H_of (pos, pos_a, pos_b)
+    print('H =', f1.H)
+
+    ##f1.update (z)
+
+    print (f1.x)
+    xs.append (f1.x[0,0]+i)
+    ys.append (f1.x[2,0]+i)
+    pxs.append (pos[0])
+    pys.append(pos[1])
+    f1.predict ()
+    print (f1.H * f1.x)
+    print (z)
+    print (f1.x)
+    f1.update(z)
+    print(f1.x)
+
+p1, = plt.plot (xs, ys, 'r--')
+p2, = plt.plot (pxs, pys)
+plt.legend([p1,p2], ['filter', 'ideal'], 2)
+plt.show()

image_tracker.py

@@ -0,0 +1,47 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Sat May 24 14:42:55 2014
+
+@author: rlabbe
+"""
+
+from KalmanFilter import KalmanFilter
+import numpy as np
+import matplotlib.pyplot as plt
+import numpy.random as random
+
+f = KalmanFilter (dim=4)
+
+dt = 1
+f.F = np.mat ([[1, dt, 0,  0],
+               [0,  1, 0,  0],
+               [0,  0, 1, dt],
+               [0,  0, 0,  1]])
+
+f.H = np.mat ([[1, 0, 0, 0],
+               [0, 0, 1, 0]])
+
+
+
+f.Q *= 4.
+f.R = np.mat([[50,0],
+              [0, 50]])
+
+f.x = np.mat([0,0,0,0]).T
+f.P *= 100.
+
+
+xs = []
+ys = []
+count = 200
+for i in range(count):
+    z = np.mat([[i+random.randn()*1],[i+random.randn()*1]])
+    f.predict ()
+    f.update (z)
+    xs.append (f.x[0,0])
+    ys.append (f.x[2,0])
+
+
+plt.plot (xs, ys)
+plt.show()
+

styles/custom2.css

@@ -4,6 +4,10 @@
         margin-left: 0% !important;
         margin-right: auto;
     }
+    div.text_cell code {
+        background: #F6F6F9;
+        color: #0000FF;
+   }
     h1 {
         font-family: 'Open sans',verdana,arial,sans-serif;
 	}
@@ -97,7 +101,7 @@
         max-height: 300px;
     }
     code{
-      font-size: 78%;
+      font-size: 70%;
     }
     .rendered_html code{
     background-color: transparent;

toc.ipynb

@@ -1,7 +1,7 @@
 {
  "metadata": {
   "name": "",
-  "signature": "sha256:678b375145397879498e1b8402d482aab5aed416480ab2c5c20431cd539c86b9"
+  "signature": "sha256:4c4932e71490dba5f12600c755687afb0d5cc008f65b6151423877e812adbbdb"
  },
  "nbformat": 3,
  "nbformat_minor": 0,
@@ -59,7 +59,7 @@
       "We extend the Kalman filter developed in the previous chapter to the full, generalized filter. \n",
       "\n",
       "\n",
-      "[**Chapter 6: Kalman Filter Math**](not implemented)\n",
+      "[**Chapter 6: Kalman Filter Math**](http://nbviewer.ipython.org/urls/raw.github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/master/Kalman_Filter_Math.ipynb)\n",
       "\n",
       "We gotten about as far as we can without forming a strong mathematical foundation. This chapter is optional, especially the first time, but if you intend to write robust, numerically stable filters, or to read the literature, you will need to know this.\n",
       "    \n",