Extensive addtions to UKF chapter.

I think I finally arrived at a good ordering of material.
Started with implementing a linear problem just so we can
see how it differs from the linear KF, then added problems
step by step. Got rid of most of the poor performing filters.

code/ukf_internal.py

@@ -9,18 +9,77 @@ from matplotlib.patches import Ellipse,Arrow
 import stats
 import numpy as np
 import math
+from filterpy.kalman import UnscentedKalmanFilter as UKF
+from stats import plot_covariance_ellipse
+
+def _sigma_points(mean, sigma, kappa):
+    sigma1 = mean + math.sqrt((1+kappa)*sigma)
+    sigma2 = mean - math.sqrt((1+kappa)*sigma)
+    return mean, sigma1, sigma2
 
 
-def arrow(x1,y1,x2,y2):
-    return Arrow(x1,y1, x2-x1, y2-y1, lw=2, width=0.1, ec='k', color='k')
+def arrow(x1,y1,x2,y2, width=0.2):
+    return Arrow(x1,y1, x2-x1, y2-y1, lw=1, width=width, ec='k', color='k')
+
+
+def show_two_sensor_bearing():
+    circle1=plt.Circle((-4,0),5,color='#004080',fill=False,linewidth=20, alpha=.7)
+    circle2=plt.Circle((4,0),5,color='#E24A33', fill=False, linewidth=5, alpha=.7)
+
+    fig = plt.gcf()
+    ax = fig.gca()
+
+    plt.axis('equal')
+    plt.xlim((-10,10))
+    plt.ylim((-10,10))
+
+    plt.plot ([-4,0], [0,3], c='#004080')
+    plt.plot ([4,0], [0,3], c='#E24A33')
+    plt.text(-4, -.5, "A", fontsize=16, horizontalalignment='center')
+    plt.text(4, -.5, "B", fontsize=16, horizontalalignment='center')
+
+    ax.add_artist(circle1)
+    ax.add_artist(circle2)
+    plt.show()
+
+def show_sigma_transform():
+    fig = plt.figure()
+    ax=fig.gca()
+
+    x = np.array([0, 5])
+    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)
+    plt.scatter(S[:,0], S[:,1], c='k', s=80)
+
+    x = np.array([15, 5])
+    P = np.array([[3, 1.2],[1.2, 6]])
+    plot_covariance_ellipse(x, P, facecolor='g', variance=9, alpha=0.5)
+
+
+    ax.add_artist(arrow(S[0,0], S[0,1], 11, 4.1, 0.6))
+    ax.add_artist(arrow(S[1,0], S[1,1], 13, 7.7, 0.6))
+    ax.add_artist(arrow(S[2,0], S[2,1], 16.3, 0.93, 0.6))
+    ax.add_artist(arrow(S[3,0], S[3,1], 16.7, 10.8, 0.6))
+    ax.add_artist(arrow(S[4,0], S[4,1], 17.7, 5.6, 0.6))
+
+    ax.axes.get_xaxis().set_visible(False)
+    ax.axes.get_yaxis().set_visible(False)
+
+    #plt.axis('equal')
+    plt.show()
+
 
 def show_2d_transform():
+
     ax=plt.gca()
 
     ax.add_artist(Ellipse(xy=(2,5), width=2, height=3,angle=70,linewidth=1,ec='k'))
     ax.add_artist(Ellipse(xy=(7,5), width=2.2, alpha=0.3, height=3.8,angle=150,linewidth=1,ec='k'))
 
     ax.add_artist(arrow(2, 5, 6, 4.8))
+
     ax.add_artist(arrow(1.5, 5.5, 7, 3.8))
     ax.add_artist(arrow(2.3, 4.1, 8, 6))
 
@@ -68,32 +127,29 @@ def show_sigma_selections():
 
 def show_sigmas_for_2_kappas():
     # generate the Gaussian data
-    
+
     xs = np.arange(-4, 4, 0.1)
     mean = 0
     sigma = 1.5
     ys = [stats.gaussian(x, mean, sigma*sigma) for x in xs]
-    
-    def sigma_points(mean, sigma, kappa):
-        sigma1 = mean + math.sqrt((1+kappa)*sigma) 
-        sigma2 = mean - math.sqrt((1+kappa)*sigma) 
-        return mean, sigma1, sigma2
-        
+
+
+
     #generate our samples
     kappa = 2
-    x0,x1,x2 = sigma_points(mean, sigma, kappa)
-    
+    x0,x1,x2 = _sigma_points(mean, sigma, kappa)
+
     samples = [x0,x1,x2]
     for x in samples:
         p1 = plt.scatter([x], [stats.gaussian(x, mean, sigma*sigma)], s=80, color='k')
-    
+
     kappa = -.5
-    x0,x1,x2 = sigma_points(mean, sigma, kappa)
-    
+    x0,x1,x2 = _sigma_points(mean, sigma, kappa)
+
     samples = [x0,x1,x2]
     for x in samples:
         p2 = plt.scatter([x], [stats.gaussian(x, mean, sigma*sigma)], s=80, color='b')
-    
+
     plt.legend([p1,p2], ['$kappa$=2', '$kappa$=-0.5'])
     plt.plot(xs, ys)
     plt.show()
@@ -101,5 +157,6 @@ def show_sigmas_for_2_kappas():
 
 
 if __name__ == '__main__':
-    show_sigma_selections()
+    show_sigma_transform()
+    #show_sigma_selections()