Added UKF/EKF comparison.

Had to adjust some examples and rerun notebooks in several places
to account for some code chantes that I made.

09_Unscented_Kalman_Filter.ipynb

@@ -3941,7 +3941,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.4.3"
+   "version": "3.4.1"
   }
  },
  "nbformat": 4,

code/nonlinear_plots.py

@@ -8,6 +8,7 @@ Created on Sun May 18 11:09:23 2014
 from __future__ import division
 import matplotlib.pyplot as plt
 import numpy as np
+from numpy.random import normal
 import scipy.stats
 
 
@@ -84,6 +85,90 @@ def plot_nonlinear_func(data, f, gaussian, num_bins=300):
     print("fuck")
 
 
+import math
+def plot_ekf_vs_mc():
+
+    def fx(x):
+        return x**3
+
+    def dfx(x):
+        return 3*x**2
+
+    mean = 1
+    var = .1
+    std = math.sqrt(var)
+
+    data = normal(loc=mean, scale=std, size=50000)
+    d_t = fx(data)
+
+    mean_ekf = fx(mean)
+
+    slope = dfx(mean)
+    std_ekf = abs(slope*std)
+
+
+    norm = scipy.stats.norm(mean_ekf, std_ekf)
+    xs = np.linspace(-3, 5, 200)
+    plt.plot(xs, norm.pdf(xs), lw=2, ls='--', color='b')
+    plt.hist(d_t, bins=200, normed=True, histtype='step', lw=2, color='g')
+
+    actual_mean = d_t.mean()
+    plt.axvline(actual_mean, lw=2, color='g', label='Monte Carlo')
+    plt.axvline(mean_ekf, lw=2, ls='--', color='b', label='EKF')
+    plt.legend()
+    plt.show()
+
+    print('actual mean={:.2f}, std={:.2f}'.format(d_t.mean(), d_t.std()))
+    print('EKF    mean={:.2f}, std={:.2f}'.format(mean_ekf, std_ekf))
+
+
+from filterpy.kalman import MerweScaledSigmaPoints, unscented_transform
+
+def plot_ukf_vs_mc(alpha=0.001, beta=3., kappa=1.):
+
+    def fx(x):
+        return x**3
+
+    def dfx(x):
+        return 3*x**2
+
+    mean = 1
+    var = .1
+    std = math.sqrt(var)
+
+    data = normal(loc=mean, scale=std, size=50000)
+    d_t = fx(data)
+
+
+    points = MerweScaledSigmaPoints(1, alpha, beta, kappa)
+    Wm, Wc = points.weights()
+    sigmas = points.sigma_points(mean, var)
+
+    sigmas_f = np.zeros((3, 1))
+    for i in range(3):
+        sigmas_f[i] = fx(sigmas[i, 0])
+
+    ### pass through unscented transform
+    ukf_mean, ukf_cov = unscented_transform(sigmas_f, Wm, Wc)
+    ukf_mean = ukf_mean[0]
+    ukf_std = math.sqrt(ukf_cov[0])
+
+    norm = scipy.stats.norm(ukf_mean, ukf_std)
+    xs = np.linspace(-3, 5, 200)
+    plt.plot(xs, norm.pdf(xs), ls='--', lw=1, color='b')
+    plt.hist(d_t, bins=200, normed=True, histtype='step', lw=1, color='g')
+
+    actual_mean = d_t.mean()
+    plt.axvline(actual_mean, lw=1, color='g', label='Monte Carlo')
+    plt.axvline(ukf_mean, lw=1, ls='--', color='b', label='UKF')
+    plt.legend()
+    plt.show()
+
+    print('actual mean={:.2f}, std={:.2f}'.format(d_t.mean(), d_t.std()))
+    print('UKF    mean={:.2f}, std={:.2f}'.format(ukf_mean, ukf_std))
+
+
+
 def test_plot():
     import math
     from numpy.random import normal
@@ -124,8 +209,9 @@ if __name__ == "__main__":
     from numpy.random import normal
     import numpy as np
 
+    plot_ukf_vs_mc()
 
-    x0 = (1, 1)
+    '''x0 = (1, 1)
     data = normal(loc=x0[0], scale=x0[1], size=500000)
 
     def g(x):
@@ -137,3 +223,4 @@ if __name__ == "__main__":
     #plot_transfer_func (data, g, lims=(-3,3), num_bins=100)
     plot_nonlinear_func (data, g, gaussian=x0,
                         num_bins=100)
+    '''

code/particle_filter.py

@@ -30,11 +30,13 @@ class ParticleFilter(object):
 
 
 
-    def create_particles(self, mu, var):
-        self.particles[:, 0] = mu[0] + randn(self.N)* np.sqrt(var)
-        self.particles[:, 1] = mu[1] + randn(self.N)* np.sqrt(var)
+    def create_particles(self, mean, variance):
+        """ create particles with the specied mean and variance"""
+        self.particles[:, 0] = mean[0] + randn(self.N) * np.sqrt(variance)
+        self.particles[:, 1] = mean[1] + randn(self.N) * np.sqrt(variance)
 
     def create_particle(self):
+        """ create particles uniformly distributed over entire space"""
         return [uniform(0, self.x_range), uniform(0, self.y_range), 0, 0]
 
 
@@ -49,23 +51,22 @@ class ParticleFilter(object):
         self.particles[:, 1] += dx[1]
 
 
-    def move(self, h, v, t=1.):
+    def move(self, hdg, vel, t=1.):
         """ move the particles according to their speed and direction for the
         specified time duration t"""
-        h = math.atan2(h[1], h[0])
+        h = math.atan2(hdg[1], hdg[0])
         h = randn(self.N) * .4 + h
-        vs = v + randn(self.N) * 0.1
-        vx = v * np.cos(h)
-        vy = v * np.sin(h)
+        vs = vel + randn(self.N) * 0.1
+        vx = vel * np.cos(h)
+        vy = vel * np.sin(h)
 
-
-        #vx = self.particles[:, 2] * np.cos(self.particles[:, 3]) + randn(self.N)*0.5
-        #vy = self.particles[:, 2] * np.sin(self.particles[:, 3]) + randn(self.N)*0.5
         self.particles[:, 0] = (self.particles[:, 0] + vx*t)
         self.particles[:, 1] = (self.particles[:, 1] + vy*t)
 
 
     def move2(self, u):
+        """ move according to control input u"""
+
         dx = u[0] + randn(self.N) * 1.9
         dy = u[1] + randn(self.N) * 1.9
         self.particles[:, 0] = (self.particles[:, 0] + dx)
@@ -87,11 +88,12 @@ class ParticleFilter(object):
         self.weights *= prob
         self.weights /= sum(self.weights) # normalize
 
+
     def neff(self):
         return 1. / np.sum(np.square(self.weights))
 
-    def resample(self):
 
+    def resample(self):
         p = np.zeros((self.N, 4))
         w = np.zeros(self.N)
 
@@ -104,6 +106,7 @@ class ParticleFilter(object):
         self.particles = p
         self.weights = w / np.sum(w)
 
+
     def estimate(self):
         """ returns mean and variance """
         pos = self.particles[:, 0:2]
@@ -140,14 +143,15 @@ def plot(pf, xlim=100, ylim=100, weights=True):
 
 if __name__ == '__main__':
     pf = ParticleFilter(5000, 100, 100)
-    pf.particles[:,3] = np.random.randn(pf.N)*np.radians(10)  + np.radians(45)
+    pf.particles[:,3] = np.random.randn(pf.N)*np.radians(10) + np.radians(45)
 
     z = np.array([20, 20])
-    pf.create_particles(z, 40)
+    pf.create_particles(mean=z, variance=40)
 
     mu0 = np.array([0., 0.])
+    plot(pf, weights=False)
 
-    for x in range(60):
+    for x in range(10):
 
         z[0] += 1.0 + randn()*0.3
         z[1] += 1.0 + randn()*0.3
@@ -170,7 +174,7 @@ if __name__ == '__main__':
         plt.scatter(mu[0], mu[1], c='g', s=100)#,
                     #s=min(500, abs((1./np.sum(var)))*20), alpha=0.5)
         plt.tight_layout()
-        plt.pause(.02)
+        plt.pause(.22)
 
         #pf.assign_speed_by_gaussian(1, 1.5)
         #pf.move(h=[1,1], v=1.4, t=1)