def plot_dog_track(xs, dog, measurement_var, process_var): N = len(xs) bp.plot_track(dog) bp.plot_measurements(xs, label='Sensor') bp.set_labels('variance = {}, process variance = {}'.format( measurement_var, process_var), 'time', 'pos') plt.ylim([0, N]) bp.show_legend() plt.show()
def plot_track(ps, actual, zs, cov, std_scale=1, plot_P=True, y_lim=None, dt=1., xlabel='time', ylabel='position', title='Kalman Filter'): count = len(zs) zs = np.asarray(zs) cov = np.asarray(cov) std = std_scale * np.sqrt(cov[:, 0, 0]) std_top = np.minimum(actual + std, [count + 10]) std_btm = np.maximum(actual - std, [-50]) std_top = actual + std std_btm = actual - std bp.plot_track(actual, c='k') bp.plot_measurements(range(1, count + 1), zs) bp.plot_filter(range(1, count + 1), ps) plt.plot(std_top, linestyle=':', color='k', lw=1, alpha=0.4) plt.plot(std_btm, linestyle=':', color='k', lw=1, alpha=0.4) plt.fill_between(range(len(std_top)), std_top, std_btm, facecolor='yellow', alpha=0.2, interpolate=True) plt.legend(loc=4) plt.xlabel(xlabel) plt.ylabel(ylabel) if y_lim is not None: plt.ylim(y_lim) else: plt.ylim((-50, count + 10)) plt.xlim((0, count)) plt.title(title) plt.show() if plot_P: ax = plt.subplot(121) ax.set_title("$\sigma^2_x$ (pos variance)") plot_covariance(cov, (0, 0)) ax = plt.subplot(122) ax.set_title("$\sigma^2_\dot{x}$ (vel variance)") plot_covariance(cov, (1, 1)) plt.show()
def plot_gh_results(weights, estimates, predictions, actual, time_step=0): n = len(weights) if time_step > 0: rng = range(1, n+1) else: rng = range(n, n+1) xs = range(n+1) book_plots.plot_measurements(xs[1:], weights, color='k', lines=False) book_plots.plot_filter(xs, estimates, marker='o', label='Estimates') book_plots.plot_track(xs[1:], predictions, c='r', marker='v', label='Predictions') plt.plot([xs[0], xs[-1]], actual, c='k', lw=1, label='Actual') plt.legend(loc=4) book_plots.set_labels(x='day', y='weight (lbs)') plt.xlim([-1, n+1]) plt.ylim([156.0, 173])
def plot_gh_results(weights, estimates, predictions, time_step=0): plt.figure(figsize=(9, 4)) n = len(weights) if time_step > 0: rng = range(1, n + 1) else: rng = range(n, n + 1) act, = book_plots.plot_track([0, n], [160, 160 + n], c='k') plt.gcf().canvas.draw() for i in rng: xs = list(range(i + 1)) pred, = book_plots.plot_track(xs[1:], predictions[:i], c='r', marker='v') plt.xlim([-1, n + 1]) plt.ylim([156.0, 173]) plt.gcf().canvas.draw() time.sleep(time_step) scale, = book_plots.plot_measurements(xs[1:], weights[:i], color='k', lines=False) plt.xlim([-1, n + 1]) plt.ylim([156.0, 173]) plt.gcf().canvas.draw() time.sleep(time_step) est, = book_plots.plot_filter(xs[:i + 1], estimates[:i + 1], marker='o') plt.xlim([-1, n + 1]) plt.ylim([156.0, 173]) plt.gcf().canvas.draw() time.sleep(time_step) plt.legend( [act, scale, est, pred], ['Actual Weight', 'Measurement', 'Estimates', 'Predictions'], loc=4) book_plots.set_labels(x='day', y='weight (lbs)') plt.xlim([-1, n + 1]) plt.ylim([156.0, 173])
def plot_track(ps, actual, zs, cov, std_scale=1, plot_P=True, y_lim=None, dt=1., xlabel='time', ylabel='position', title='Kalman Filter'): count = len(zs) zs = np.asarray(zs) cov = np.asarray(cov) std = std_scale*np.sqrt(cov[:,0,0]) std_top = np.minimum(actual+std, [count + 10]) std_btm = np.maximum(actual-std, [-50]) std_top = actual + std std_btm = actual - std bp.plot_track(actual,c='k') bp.plot_measurements(range(1, count + 1), zs) bp.plot_filter(range(1, count + 1), ps) plt.plot(std_top, linestyle=':', color='k', lw=1, alpha=0.4) plt.plot(std_btm, linestyle=':', color='k', lw=1, alpha=0.4) plt.fill_between(range(len(std_top)), std_top, std_btm, facecolor='yellow', alpha=0.2, interpolate=True) plt.legend(loc=4) plt.xlabel(xlabel) plt.ylabel(ylabel) if y_lim is not None: plt.ylim(y_lim) else: plt.ylim((-50, count + 10)) plt.xlim((0,count)) plt.title(title) plt.show() if plot_P: ax = plt.subplot(121) ax.set_title("$\sigma^2_x$ (pos variance)") plot_covariance(cov, (0, 0)) ax = plt.subplot(122) ax.set_title("$\sigma^2_\dot{x}$ (vel variance)") plot_covariance(cov, (1, 1)) plt.show()
def plot_gh_results(weights, estimates, predictions, time_step=0): n = len(weights) if time_step > 0: rng = range(1, n+1) else: rng = range(n, n+1) plt.xlim([-1, n+1]) plt.ylim([156.0, 173]) act, = book_plots.plot_track([0, n], [160, 160+n], c='k') plt.gcf().canvas.draw() for i in rng: xs = list(range(i+1)) #plt.cla() pred, = book_plots.plot_track(xs[1:], predictions[:i], c='r', marker='v') plt.xlim([-1, n+1]) plt.ylim([156.0, 173]) plt.gcf().canvas.draw() time.sleep(time_step) scale, = book_plots.plot_measurements(xs[1:], weights[:i], color='k', lines=False) plt.xlim([-1, n+1]) plt.ylim([156.0, 173]) plt.gcf().canvas.draw() time.sleep(time_step) est, = book_plots.plot_filter(xs[:i+1], estimates[:i+1], marker='o') plt.xlim([-1, n+1]) plt.ylim([156.0, 173]) plt.gcf().canvas.draw() time.sleep(time_step) plt.legend([act, scale, est, pred], ['Actual Weight', 'Measurement', 'Estimates', 'Predictions'], loc=4) book_plots.set_labels(x='day', y='weight (lbs)')
def plot_gh_results(weights, estimates, predictions, time_step=0): n = len(weights) if time_step > 0: rng = range(1, n+1) else: rng = range(n, n+1) xs = range(n+1) pred, = book_plots.plot_track(xs[1:], predictions, c='r', marker='v') scale, = book_plots.plot_measurements(xs[1:], weights, color='k', lines=False) est, = book_plots.plot_filter(xs, estimates, marker='o') plt.legend([scale, est, pred], ['Measurement', 'Estimates', 'Predictions'], loc=4) book_plots.set_labels(x='day', y='weight (lbs)') plt.xlim([-1, n+1]) plt.ylim([156.0, 173])
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()
'data' contains the data(readings from the sensor) to be filtered. 'x0' is the initial value for our state variable 'dx' is the initial change rate for our state variable 'g' is the g-h's g scale factor 'h' is the g-h's h scale factor 'dt' is the length of the time step """ x = x0 meas = [] for z in data: # predict predict = x + dx * dt # update residual = z - predict x = predict + g * residual dx = dx + h * residual / dt meas.append(x) return np.array(meas) %matplotlib inline weights = [158.0, 164.2, 160.3, 159.9, 162.1, 164.6, 169.6, 167.4, 166.4, 171.0, 171.2, 172.6] plt.figure(figsize=(16, 9)) book_plots.plot_track([0, 11], [160, 172], label='Actual weight') data = g_h_filter(data=weights, x0=160., dx=1., g=6./10, h=2./3, dt=1.) plot_g_h_results(weights, data)