Working for UKF rename.

Most of the text is wrong, but changed code to use the
renamed ScaledUnscentedKalmanFilter.

Checking in with bad text because I am in the process of changing
FilterPy to use a class for the sigma points to make it easier
to change the sigma point generation, leaving us with one
UKF class instead of several.

book_format.py

@@ -62,6 +62,12 @@ def numpy_precision(precision):
 	yield
 	np.set_printoptions(old)
 	
+@contextmanager
+def printoptions(*args, **kwargs):
+    original = np.get_printoptions()
+    np.set_printoptions(*args, **kwargs)
+    yield 
+    np.set_printoptions(**original)
 	
 def _decode_list(data):
     rv = []

code/ukf_internal.py

@@ -85,7 +85,7 @@ def show_sigma_transform():
     P = np.array([[4, -2.2], [-2.2, 3]])
 
     plot_covariance_ellipse(x, P, facecolor='b', variance=9, alpha=0.5)
-    S = UKF.sigma_points(x=x, P=P, kappa=0)
+    S = UKF.sigma_points(x=x, P=P, alpha=.5, kappa=0)
     plt.scatter(S[:,0], S[:,1], c='k', s=80)
 
     x = np.array([15, 5])

experiments/ekf4.py

@@ -17,7 +17,6 @@ from math import cos, sin, sqrt, atan2, tan
 import matplotlib.pyplot as plt
 import numpy as np
 from numpy import array, dot
-from numpy.linalg import pinv
 from numpy.random import randn
 from filterpy.common import plot_covariance_ellipse
 from filterpy.kalman import ExtendedKalmanFilter as EKF
@@ -44,8 +43,6 @@ sigma_r = 1
 sigma_h =  .1#np.radians(1)
 sigma_steer =  np.radians(1)
 
-#only partway through. predict is using old control and movement code. computation of m uses
-#old u.
 
 class RobotEKF(EKF):
     def __init__(self, dt, wheelbase):
@@ -122,7 +119,7 @@ class RobotEKF(EKF):
 
 def H_of(x, p):
     """ compute Jacobian of H matrix where h(x) computes the range and
-    bearing to a landmark for state x """
+    bearing to a landmark 'p' for state x """
 
     px = p[0]
     py = p[1]
@@ -157,15 +154,16 @@ m = array([[5, 10],
            [15, 15]])
 
 ekf.x = array([[2, 6, .3]]).T
-u = array([1.1, .01])
 ekf.P = np.diag([.1, .1, .1])
 ekf.R = np.diag([sigma_r**2, sigma_h**2])
-c = [0, 1, 2]
+
+u = array([1.1, .01])
 
 xp = ekf.x.copy()
 
+plt.figure()
 plt.scatter(m[:, 0], m[:, 1])
-for i in range(150):
+for i in range(250):
     xp = ekf.move(xp, u, dt/10.) # simulate robot
     plt.plot(xp[0], xp[1], ',', color='g')
 
@@ -173,7 +171,7 @@ for i in range(150):
 
         ekf.predict(u=u)
 
-        plot_covariance_ellipse((ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2], std=10,
+        plot_covariance_ellipse((ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2], std=3,
                                 facecolor='b', alpha=0.08)
 
         for lmark in m:
@@ -184,11 +182,12 @@ for i in range(150):
             ekf.update(z, HJacobian=H_of, Hx=Hx, residual=residual,
                        args=(lmark), hx_args=(lmark))
 
-        plot_covariance_ellipse((ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2], std=10,
+        plot_covariance_ellipse((ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2], std=3,
                                 facecolor='g', alpha=0.4)
 
     #plt.plot(ekf.x[0], ekf.x[1], 'x', color='r')
 
 plt.axis('equal')
+plt.title("EKF Robot localization")
 plt.show()
 

experiments/ukfloc.py

@@ -0,0 +1,207 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Mon Jun  1 18:13:23 2015
+
+@author: rlabbe
+"""
+
+from filterpy.common import plot_covariance_ellipse
+from filterpy.kalman import ScaledUnscentedKalmanFilter as UKF
+from math import tan, sin, cos, sqrt, atan2
+import matplotlib.pyplot as plt
+from numpy import array
+import numpy as np
+from numpy.random import randn
+
+
+
+def normalize_angle(x, index):
+    def normalize(x):
+        if x > np.pi:
+            x -= 2*np.pi
+        if x < -np.pi:
+            x = 2*np.pi
+        return x
+
+    if x.ndim > 1:
+        for i in range(len(x)):
+            x[i, index] = normalize(x[i, index])
+
+    else:
+        x[index] = normalize(x[index])
+
+
+def residual(a,b , index=1):
+    y = a - b
+    normalize_angle(y, index)
+    return y
+
+
+def residual_h(a, b):
+    return residual(a, b, 1)
+
+def residual_x(a, b):
+    return residual(a, b, 2)
+
+
+def move(x, u, dt, wheelbase):
+    h = x[2]
+    v = u[0]
+    steering_angle = u[1]
+
+    dist = v*dt
+
+    if abs(steering_angle) < 0.0001:
+        # approximate straight line with huge radius
+        r = 1.e-30
+    b = dist / wheelbase * tan(steering_angle)
+    r = wheelbase / tan(steering_angle) # radius
+
+
+    sinh = sin(h)
+    sinhb = sin(h + b)
+    cosh = cos(h)
+    coshb = cos(h + b)
+
+    return x + array([-r*sinh + r*sinhb, r*cosh - r*coshb, b])
+
+
+
+
+
+def state_unscented_transform(Sigmas, Wm, Wc, noise_cov):
+    """ Computes unscented transform of a set of sigma points and weights.
+    returns the mean and covariance in a tuple.
+    """
+
+    kmax, n = Sigmas.shape
+
+    x = np.zeros(3)
+    sum_sin, sum_cos = 0., 0.
+
+    for i in range(len(Sigmas)):
+        s = Sigmas[i]
+        x[0] += s[0] * Wm[i]
+        x[1] += s[1] * Wm[i]
+        sum_sin += sin(s[2])*Wm[i]
+        sum_cos += cos(s[2])*Wm[i]
+
+    x[2] = atan2(sum_sin, sum_cos)
+
+
+    # new covariance is the sum of the outer product of the residuals
+    # times the weights
+    P = np. zeros((n, n))
+    for k in range(kmax):
+        y = residual_x(Sigmas[k],  x)
+        P += Wc[k] * np.outer(y, y)
+
+    if noise_cov is not None:
+        P += noise_cov
+
+    return (x, P)
+
+
+
+def z_unscented_transform(Sigmas, Wm, Wc, noise_cov):
+    """ Computes unscented transform of a set of sigma points and weights.
+    returns the mean and covariance in a tuple.
+    """
+
+    kmax, n = Sigmas.shape
+
+    x = np.zeros(2)
+    sum_sin, sum_cos = 0., 0.
+
+    for i in range(len(Sigmas)):
+        s = Sigmas[i]
+        x[0] += s[0] * Wm[i]
+        sum_sin += sin(s[1])*Wm[i]
+        sum_cos += cos(s[1])*Wm[i]
+
+    x[1] = atan2(sum_sin, sum_cos)
+
+
+    # new covariance is the sum of the outer product of the residuals
+    # times the weights
+    P = np.zeros((n, n))
+    for k in range(kmax):
+        y = residual_h(Sigmas[k], x)
+
+        P += Wc[k] * np.outer(y, y)
+
+
+    if noise_cov is not None:
+        P += noise_cov
+
+    return (x, P)
+
+
+sigma_r = 1.
+sigma_h =  .1#np.radians(1)
+sigma_steer =  np.radians(.01)
+dt = 1.0
+wheelbase = 0.5
+
+m = array([[5, 10],
+           [10, 5],
+           [15, 15]])
+
+
+def fx(x, dt, u):
+    return move(x, u, dt, wheelbase)
+
+
+def Hx(x, landmark):
+    """ takes a state variable and returns the measurement that would
+    correspond to that state.
+    """
+    px = landmark[0]
+    py = landmark[1]
+    dist = np.sqrt((px - x[0])**2 + (py - x[1])**2)
+
+    Hx = array([dist, atan2(py - x[1], px - x[0]) - x[2]])
+    return Hx
+
+
+ukf= UKF(dim_x=3, dim_z=2, fx=fx, hx=Hx, dt=dt, alpha=1.e-3, beta=.1, kappa=0)
+ukf.x = array([2, 6, .3])
+ukf.P = np.diag([.1, .1, .2])
+ukf.R = np.diag([sigma_r**2, sigma_h**2])
+ukf.Q = np.zeros((3,3))
+
+
+u = array([1.1, .01])
+
+xp = ukf.x.copy()
+
+plt.figure()
+plt.scatter(m[:, 0], m[:, 1])
+
+for i in range(250):
+    xp = move(xp, u, dt/10., wheelbase) # simulate robot
+    plt.plot(xp[0], xp[1], ',', color='g')
+
+    if i % 10 == 0:
+        ukf.predict(fx_args=u, UT=state_unscented_transform)
+
+        plot_covariance_ellipse((ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=3,
+                                facecolor='b', alpha=0.08)
+
+        for lmark in m:
+            d = sqrt((lmark[0] - xp[0])**2 + (lmark[1] - xp[1])**2)  + randn()*sigma_r
+            a = atan2(lmark[1] - xp[1], lmark[0] - xp[0]) - xp[2] + randn()*sigma_h
+            z = np.array([d, a])
+
+            ukf.update(z, hx_args=(lmark,), UT=z_unscented_transform,
+                       residual_x=residual_x, residual_h=residual_h)
+
+        plot_covariance_ellipse((ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=3,
+                                facecolor='g', alpha=0.4)
+
+
+    #plt.plot(ekf.x[0], ekf.x[1], 'x', color='r')
+
+plt.axis('equal')
+plt.title("UKF Robot localization")
+plt.show()