refactor to minimize code plot size.

Added code to set x,y labels and title, and to set xlim, ylim
in one line.

Also moved some plotting code to the *internal.py files.

code/book_plots.py

@@ -39,6 +39,26 @@ def bar_plot(pos, ylim=(0,1), title=None):
         plt.title(title)
 
 
+def set_labels(title=None, x=None, y=None):
+    """ helps make code in book shorter. Optional set title, xlabel and ylabel
+    """
+    if x is not None:
+        plt.xlabel(x)
+    if y is not None:
+        plt.ylabel(y)
+    if title is not None:
+        plt.title(title)
+
+
+def set_limits(x, y):
+    """ helper function to make code in book shorter. Set the limits for the x
+    and y axis.
+    """
+
+    plt.gca().set_xlim(x)
+    plt.gca().set_ylim(y)
+
+
 def plot_measurements(xs, ys=None, color='r', lw=2, label='Measurements', **kwargs):
     """ Helper function to give a consistant way to display
     measurements in the book.

code/ekf_internal.py

@@ -4,6 +4,8 @@ Created on Fri Jul 18 23:23:08 2014
 
 @author: rlabbe
 """
+
+import book_plots as bp
 from math import radians, sin, cos, sqrt, exp
 import numpy.random as random
 import matplotlib.pyplot as plt
@@ -38,6 +40,31 @@ def ball_kf(x, y, omega, v0, dt, r=0.5, q=0.02):
     return f1
 
 
+def plot_radar(xs, track, time):
+
+    plt.figure()
+    bp.plot_track(time, track[:, 0])
+    bp.plot_filter(time, xs[:, 0])
+    plt.legend(loc=4)
+    plt.xlabel('time (sec)')
+    plt.ylabel('position (m)')
+
+    plt.figure()
+    bp.plot_track(time, track[:, 1])
+    bp.plot_filter(time, xs[:, 1])
+    plt.legend(loc=4)
+    plt.xlabel('time (sec)')
+    plt.ylabel('velocity (m/s)')
+
+    plt.figure()
+    bp.plot_track(time, track[:, 2])
+    bp.plot_filter(time, xs[:, 2])
+    plt.ylabel('altitude (m)')
+    plt.legend(loc=4)
+    plt.xlabel('time (sec)')
+    plt.ylim((900, 1600))
+    plt.show()
+
 def plot_bicycle():
     plt.clf()
     plt.axes()

code/nonlinear_internal.py

@@ -0,0 +1,50 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Thu Jul  9 13:02:32 2015
+
+@author: Roger Labbe
+"""
+
+import filterpy.stats as stats
+import matplotlib.pyplot as plt
+from matplotlib.patches import Ellipse
+import numpy as np
+
+
+def plot1():
+
+    stats.plot_covariance_ellipse((10, 2), P, facecolor='g', alpha=0.2)
+
+
+def plot2():
+    P = np.array([[6, 2.5], [2.5, .6]])
+    circle1=plt.Circle((10,0),3,color='#004080',fill=False,linewidth=4, alpha=.7)
+    ax = plt.gca()
+    ax.add_artist(circle1)
+    plt.xlim(0,10)
+    plt.ylim(0,3)
+    P = np.array([[6, 2.5], [2.5, .6]])
+    stats.plot_covariance_ellipse((10, 2), P, facecolor='g', alpha=0.2)
+
+def plot3():
+    P = np.array([[6, 2.5], [2.5, .6]])
+    circle1=plt.Circle((10,0),3,color='#004080',fill=False,linewidth=4, alpha=.7)
+    ax = plt.gca()
+    ax.add_artist(circle1)
+    plt.xlim(0,10)
+    plt.ylim(0,3)
+    plt.axhline(3, ls='--')
+    stats.plot_covariance_ellipse((10, 2), P,  facecolor='g', alpha=0.2)
+
+def plot4():
+    P = np.array([[6, 2.5], [2.5, .6]])
+    circle1=plt.Circle((10,0),3,color='#004080',fill=False,linewidth=4, alpha=.7)
+    ax = plt.gca()
+    ax.add_artist(circle1)
+    plt.xlim(0,10)
+    plt.ylim(0,3)
+    plt.axhline(3, ls='--')
+    stats.plot_covariance_ellipse((10, 2), P,  facecolor='g', alpha=0.2)
+    plt.scatter([11.4], [2.65],s=200)
+    plt.scatter([12], [3], c='r', s=200)
+    plt.show()

code/ukf_internal.py

@@ -236,6 +236,62 @@ def plot_sigma_points():
     plt.show()
     print(sum(Wc0))
 
+def plot_radar(xs, t, plot_x=True, plot_vel=True, plot_alt=True):
+    xs = np.asarray(xs)
+    if plot_x:
+        plt.figure()
+        plt.plot(t, xs[:, 0]/1000.)
+        plt.xlabel('time(sec)')
+        plt.ylabel('position(km)')
+    if plot_vel:
+        plt.figure()
+        plt.plot(t, xs[:, 1])
+        plt.xlabel('time(sec)')
+        plt.ylabel('velocity')
+    if plot_alt:
+        plt.figure()
+        plt.plot(t, xs[:,2])
+        plt.xlabel('time(sec)')
+        plt.ylabel('altitude')
+    plt.show()
+
+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()
+    print('mean weights:', Wm)
+    print('cov weights:', Wc)
+    print('lambda:', alpha**2 *(n+kappa) - n)
+    print('sum cov', sum(Wc))
+
+
+def plot_rts_output(xs, Ms, t):
+    plt.figure()
+    plt.plot(t, xs[:, 0]/1000., label='KF', lw=2)
+    plt.plot(t, Ms[:, 0]/1000., c='k', label='RTS', lw=2)
+    plt.xlabel('time(sec)')
+    plt.ylabel('x')
+    plt.legend(loc=4)
+
+    plt.figure()
+
+    plt.plot(t, xs[:, 1], label='KF')
+    plt.plot(t, Ms[:, 1], c='k', label='RTS')
+    plt.xlabel('time(sec)')
+    plt.ylabel('x velocity')
+    plt.legend(loc=4)
+
+    plt.figure()
+    plt.plot(t, xs[:, 2], label='KF')
+    plt.plot(t, Ms[:, 2], c='k', label='RTS')
+    plt.xlabel('time(sec)')
+    plt.ylabel('Altitude(m)')
+    plt.legend(loc=4)
+
+    np.set_printoptions(precision=4)
+    print('Difference in position in meters:', xs[-6:-1, 0] - Ms[-6:-1, 0])
+
+
 if __name__ == '__main__':
 
     #show_2d_transform()