Extensive copy editing.

07-Kalman-Filter-Math.ipynb

@@ -349,7 +349,7 @@
    "cell_type": "markdown",
    "metadata": {},
    "source": [
-    "## State Space Representations of Dynamic Systems"
+    "## State-Space Representations of Dynamic Systems"
    ]
   },
   {
@@ -358,7 +358,7 @@
    "source": [
     "The equation $\\mathbf{v} = \\frac{d \\mathbf{x}}{d t}$ is the simplest possible differential equation. We trivially integrate it into the high school physics equation $x = vt + x_0$. Almost all other differential equations encountered in physical systems will not yield to this approach. \n",
     "\n",
-    "*State space* methods became popular around the time of the Apollo missions, in no small part due to the work of Dr. Kalman. The idea is simple. First, we convert a system of $n^{th}$-order differential equations into an equivalent set of first-order differential systems. We can then represent that set of first order equations as in vector matrix form. Once in this form we use the formidable powers of linear algebra to solve the system of equations. The Kalman filter is an example of this power. "
+    "*State-space* methods became popular around the time of the Apollo missions, in no small part due to the work of Dr. Kalman. The idea is simple. First, we convert a system of $n^{th}$-order differential equations into an equivalent set of first-order differential systems. We can then represent that set of first order equations as in vector matrix form. Once in this form we use the formidable powers of linear algebra to solve the system of equations. The Kalman filter is an example of this power. "
    ]
   },
   {
@@ -449,7 +449,7 @@
     "\n",
     "In other words, we need to find the inverse of $F$. This is not at all trivial, and a significant amount of coursework in a STEM education is devoted to finding tricky, analytic solutions to this problem. \n",
     "\n",
-    "In the end, however, they are tricks, and many simple forms of $f(x)$ either have no closed form solution or pose extreme difficulties. Instead, the practicing engineer turns to state space methods to find solutions."
+    "In the end, however, they are tricks, and many simple forms of $f(x)$ either have no closed form solution or pose extreme difficulties. Instead, the practicing engineer turns to state-space methods to find solutions."
    ]
   },
   {
@@ -458,7 +458,7 @@
    "source": [
     "### Forming First Order Equations from Higher Order Equations\n",
     "\n",
-    "Models of physical systems often require second or higher order equations. However, state space methods require first-order differential equations, which are equations with only first derivatives. Any higher order system of equations can be converted to a first order set of equations by defining extra variables for the first order terms and then solving. \n",
+    "Models of physical systems often require second or higher order equations. However, state-space methods require first-order differential equations, which are equations with only first derivatives. Any higher order system of equations can be converted to a first order set of equations by defining extra variables for the first order terms and then solving. \n",
     "\n",
     "Let's do an example. Given the system $\\ddot{x} - 6\\dot{x} + 9x = t$ find the first order equations.\n",
     "\n",
@@ -589,7 +589,7 @@
     "\n",
     "$$ v = \\dot{x}\\\\a=\\ddot{x} =0,$$\n",
     "\n",
-    "which we can put in state space matrix form as\n",
+    "which we can put in state-space matrix form as\n",
     "\n",
     "$$\\begin{bmatrix}\\dot{x} \\\\ \\ddot{x}\\end{bmatrix} =\\begin{bmatrix}0&1\\\\0&0\\end{bmatrix} \\begin{bmatrix}x \\\\ \\dot{x}\\end{bmatrix}$$\n",
     "\n",
@@ -619,7 +619,7 @@
     "This should look very familiar to you! This is the equation we used in the **Multivariate Kalman Filter** chapter to track a moving object.\n",
     "\n",
     "$$\n",
-    "{\\begin{bmatrix}x\\\\\\dot{x}\\end{bmatrix}}^- =\\begin{bmatrix}1&t \\\\ 0&1\\end{bmatrix} \\begin{bmatrix}x \\\\ \\dot{x}\\end{bmatrix}\n",
+    "{\\begin{bmatrix}x\\\\\\dot{x}\\end{bmatrix}}^- =\\begin{bmatrix}1&\\Delta t \\\\ 0&1\\end{bmatrix} \\begin{bmatrix}x \\\\ \\dot{x}\\end{bmatrix}\n",
     "$$\n",
     "\n",
     "We derived this equation in that chapter by using techniques that are much easier to understand. The advantage of the Taylor series expansion is that we can use it for any arbitrary set of differential equations which are time invariant. \n",

code/ekf_internal.py

@@ -52,7 +52,6 @@ def ball_kf(x, y, omega, v0, dt, r=0.5, q=0.02):
 
 
 def plot_radar(xs, track, time):
-
     plt.figure()
     bp.plot_track(time, track[:, 0])
     bp.plot_filter(time, xs[:, 0])
@@ -76,6 +75,7 @@ def plot_radar(xs, track, time):
     plt.ylim((900, 1600))
     plt.show()
 
+    
 def plot_bicycle():
     plt.clf()
     plt.axes()
@@ -215,11 +215,11 @@ def show_radar_chart():
 
 
 
-    ax.annotate('$\Theta$', xy=(1.2, 1.1), color='b')
+    ax.annotate('$\Theta$', xy=(1.2, 1.05), color='b')
     ax.annotate('Aircraft', xy=(2.04,2.), color='b')
-    ax.annotate('altitude', xy=(2.04,1.5), color='k')
-    ax.annotate('X', xy=(1.5, .9))
-    ax.annotate('Radar', xy=(.95, 0.9))
+    ax.annotate('altitude (y)', xy=(2.04,1.5), color='k')
+    ax.annotate('x', xy=(1.5, .9))
+    ax.annotate('Radar', xy=(.95, 0.8))
     ax.annotate('Slant\n  (r)', xy=(1.5,1.62), color='r')
 
     plt.title("Radar Tracking")
@@ -229,4 +229,4 @@ def show_radar_chart():
     ax.yaxis.set_ticks([])
 
 
-    plt.show()
+    plt.show()

code/nonlinear_plots.py

@@ -16,6 +16,7 @@ for more information.
 from __future__ import (absolute_import, division, print_function,
                         unicode_literals)
 
+from filterpy.kalman import MerweScaledSigmaPoints, unscented_transform
 from filterpy.stats import multivariate_gaussian
 from matplotlib import cm
 import matplotlib.pyplot as plt
@@ -47,10 +48,10 @@ def plot_nonlinear_func(data, f, gaussian, num_bins=300):
     #plot output
     h = np.histogram(ys, num_bins, density=False)
     plt.subplot(2,2,4)
-    plt.plot(h[0], h[1][1:], lw=4, alpha=0.5)
+    plt.plot(h[0], h[1][1:], lw=4, alpha=0.8)
     plt.ylim(out_lims[1], out_lims[0])
     plt.gca().xaxis.set_ticklabels([])
-    plt.title('output')
+    plt.title('Output')
 
     plt.axhline(np.mean(ys), ls='--', lw=2)
     plt.axhline(f(x0), lw=1)
@@ -66,16 +67,16 @@ def plot_nonlinear_func(data, f, gaussian, num_bins=300):
     print(max(norm.pdf(xs)))'''
 
     # plot transfer function
-    plt.subplot(2,2,3)
+    plt.subplot(2, 2, 3)
     x = np.arange(in_lims[0], in_lims[1], 0.1)
     y = f(x)
-    plt.plot (x,y, 'k')
+    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.xlim(in_lims)
     plt.ylim(out_lims)
     #plt.axis('equal')
-    plt.title('function')
+    plt.title('f(x)')
 
     # plot input
     h = np.histogram(data, num_bins, density=True)
@@ -84,7 +85,7 @@ def plot_nonlinear_func(data, f, gaussian, num_bins=300):
     plt.plot(h[1][1:], h[0], lw=4)
     plt.xlim(in_lims)
     plt.gca().yaxis.set_ticklabels([])
-    plt.title('input')
+    plt.title('Input')
 
     plt.show()
 
@@ -106,11 +107,9 @@ def plot_ekf_vs_mc():
     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')
@@ -126,8 +125,6 @@ def plot_ekf_vs_mc():
     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):
@@ -159,12 +156,12 @@ def plot_ukf_vs_mc(alpha=0.001, beta=3., kappa=1.):
 
     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')
+    plt.plot(xs, norm.pdf(xs), ls='--', lw=2, 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=1, color='g', label='Monte Carlo')
-    plt.axvline(ukf_mean, lw=1, ls='--', color='b', label='UKF')
+    plt.axvline(actual_mean, lw=2, color='g', label='Monte Carlo')
+    plt.axvline(ukf_mean, lw=2, ls='--', color='b', label='UKF')
     plt.legend()
     plt.show()
 

code/pf_internal.py

@@ -118,6 +118,7 @@ def plot_random_pd():
         #plt.setp(plt.gca().get_yticklabels(), visible=False)
         plt.axes(xticks=[], yticks=[], frameon=False)
         plt.plot(x, y2)
+        plt.ylim([0, max(y2)+.1])
 
 
 def plot_monte_carlo_ukf():

code/ukf_internal.py

@@ -279,6 +279,17 @@ def plot_radar(xs, t, plot_x=True, plot_vel=True, plot_alt=True):
         plt.ylabel('altitude')
     plt.show()
 
+    
+def plot_altitude(xs, t, track):
+    xs = np.asarray(xs)
+
+    plt.plot(t, xs[:,2], label='filter', )
+    plt.plot(t, track, label='Aircraft', lw=2, ls='--', c='k')
+    plt.xlabel('time(sec)')
+    plt.ylabel('altitude')
+    plt.legend(loc=4)
+
+    
 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])