Updated for FilterPy 1.4.0

kf_book/nonlinear_plots.py

@@ -28,34 +28,31 @@ from filterpy.kalman import MerweScaledSigmaPoints, unscented_transform
 from filterpy.stats import multivariate_gaussian
 
 
-def plot_nonlinear_func(data, f, gaussian, num_bins=300):
-    # linearize at mean to simulate EKF
-    #x = gaussian[0]
+def plot_nonlinear_func(data, f, out_lim=None, num_bins=300):
 
-    # equation of linearization
-    #m = df(x)
-    #b = f(x) - x*m
-
-    # compute new mean and variance based on EKF equations
     ys = f(data)
-    x0 = gaussian[0]
-    in_std = np.sqrt(gaussian[1])
+    x0 = np.mean(data)
+    in_std = np.std(data)
+    
     y = f(x0)
     std = np.std(ys)
 
-    in_lims = [x0-in_std*3, x0+in_std*3]
-    out_lims = [y-std*3, y+std*3]
+    in_lims = [x0 - in_std*3, x0 + in_std*3]
+    if out_lim is None:
+        out_lim = [y - std*3, y + std*3]
 
-    #plot output
+    # plot output
     h = np.histogram(ys, num_bins, density=False)
-    plt.subplot(2,2,4)
-    plt.plot(h[0], h[1][1:], lw=2, alpha=0.8)
-    plt.ylim(out_lims[1], out_lims[0])
-    plt.gca().xaxis.set_ticklabels([])
+    plt.subplot(221)
+    plt.plot(h[1][1:], h[0], lw=2, alpha=0.8)
+    if out_lim is not None:
+        plt.xlim(out_lim[0], out_lim[1])
+
+    plt.gca().yaxis.set_ticklabels([])
     plt.title('Output')
 
-    plt.axhline(np.mean(ys), ls='--', lw=2)
-    plt.axhline(f(x0), lw=1)
+    plt.axvline(np.mean(ys), ls='--', lw=2)
+    plt.axvline(f(x0), lw=1)
 
 
     norm = scipy.stats.norm(y, in_std)
@@ -73,20 +70,21 @@ def plot_nonlinear_func(data, f, gaussian, num_bins=300):
     y = f(x)
     plt.plot (x, y, 'k')
     isct = f(x0)
-    plt.plot([x0, x0, in_lims[1]], [out_lims[1], isct, isct], color='r', lw=1)
+    plt.plot([x0, x0, in_lims[1]], [out_lim[1], isct, isct], color='r', lw=1)
     plt.xlim(in_lims)
-    plt.ylim(out_lims)
+    plt.ylim(out_lim)
     #plt.axis('equal')
     plt.title('f(x)')
 
     # plot input
     h = np.histogram(data, num_bins, density=True)
 
-    plt.subplot(2,2,1)
-    plt.plot(h[1][1:], h[0], lw=2)
-    plt.xlim(in_lims)
-    plt.gca().yaxis.set_ticklabels([])
+    plt.subplot(2,2,4)
+    plt.plot(h[0], h[1][1:], lw=2)
+    #plt.ylim(in_lims)
+    plt.gca().xaxis.set_ticklabels([])
     plt.title('Input')
+    plt.tight_layout()
     plt.show()
 
     
@@ -141,7 +139,7 @@ def plot_ukf_vs_mc(alpha=0.001, beta=3., kappa=1.):
 
 
     points = MerweScaledSigmaPoints(1, alpha, beta, kappa)
-    Wm, Wc = points.weights()
+    Wm, Wc = points.Wm, points.Wc
     sigmas = points.sigma_points(mean, var)
 
     sigmas_f = np.zeros((3, 1))

kf_book/ukf_internal.py

@@ -179,21 +179,21 @@ def show_sigma_selections():
 
     points = MerweScaledSigmaPoints(2, .09, 2., 1.)
     sigmas = points.sigma_points(x, P)
-    Wm, Wc = points.weights()
+    Wm, Wc = points.Wm, points.Wc
     plot_covariance_ellipse(x, P, facecolor='b', alpha=.3, variance=[.5])
     _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k')
 
     x = np.array([5, 5])
     points = MerweScaledSigmaPoints(2, .15, 1., .15)
     sigmas = points.sigma_points(x, P)
-    Wm, Wc = points.weights()
+    Wm, Wc = points.Wm, points.Wc
     plot_covariance_ellipse(x, P, facecolor='b', alpha=0.3, variance=[.5])
     _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k')
 
     x = np.array([8, 5])
     points = MerweScaledSigmaPoints(2, .2, 3., 10)
     sigmas = points.sigma_points(x, P)
-    Wm, Wc = points.weights()
+    Wm, Wc = points.Wm, points.Wc
     plot_covariance_ellipse(x, P, facecolor='b', alpha=0.3, variance=[.5])
     _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k')
 
@@ -232,17 +232,25 @@ def show_sigmas_for_2_kappas():
     plt.show()
 
 
+def plot_sigmas(sigmas, x, cov):
+    if not np.isscalar(cov):
+        cov = np.atleast_2d(cov)
+    pts = sigmas.sigma_points(x=x, P=cov)
+    plt.scatter(pts[:, 0], pts[:, 1], s=sigmas.Wm*1000)
+    plt.axis('equal')
+    
+
 def plot_sigma_points():
     x = np.array([0, 0])
     P = np.array([[4, 2], [2, 4]])
 
     sigmas = MerweScaledSigmaPoints(n=2, alpha=.3, beta=2., kappa=1.)
     S0 = sigmas.sigma_points(x, P)
-    Wm0, Wc0 = sigmas.weights()
+    Wm0, Wc0 = sigmas.Wm, sigmas.Wc
 
     sigmas = MerweScaledSigmaPoints(n=2, alpha=1., beta=2., kappa=1.)
     S1 = sigmas.sigma_points(x, P)
-    Wm1, Wc1 = sigmas.weights()
+    Wm1, Wc1 = sigmas.Wm, sigmas.Wc
 
     def plot_sigmas(s, w, **kwargs):
         min_w = min(abs(w))
@@ -296,7 +304,7 @@ def plot_altitude(xs, t, track):
 def print_sigmas(n=1, mean=5, cov=3, alpha=.1, beta=2., kappa=2):
     points = MerweScaledSigmaPoints(n, alpha, beta, kappa)
     print('sigmas: ', points.sigma_points(mean,  cov).T[0])
-    Wm, Wc = points.weights()
+    Wm, Wc = points.Wm, points.Wc
     print('mean weights:', Wm)
     print('cov weights:', Wc)
     print('lambda:', alpha**2 *(n+kappa) - n)