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_track_and_residuals(t, xs, z_xs, res): plt.subplot(121) if z_xs is not None: bp.plot_measurements(t, z_xs, label='z') bp.plot_filter(t, xs) plt.legend(loc=2) plt.xlabel('time (sec)') plt.ylabel('X') plt.title('estimates vs measurements') plt.subplot(122) # plot twice so it has the same color as the plot to the left! plt.plot(t, res) plt.plot(t, res) plt.xlabel('time (sec)') plt.ylabel('residual') plt.title('residuals') plt.show()
def plot_track_and_residuals(t, xs, z_xs, res): plt.subplot(121) if z_xs is not None: bp.plot_measurements(t, z_xs, label='z') bp.plot_filter(t, xs) plt.legend(loc=2) plt.xlabel('time (sec)') plt.ylabel('X') plt.title('estimates vs measurements') plt.subplot(122) # plot twice so it has the same color as the plot to the left! plt.plot(t, res) plt.plot(t, res) plt.xlabel('time (sec)') plt.ylabel('residual') plt.title('residuals') 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) 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, pred], ['Actual Weight', 'Measurement', 'Predictions'], loc=4) book_plots.set_labels(x='day', y='weight (lbs)')
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_g_h_results(measurements, filtered_data, title='', z_label='Measurements', **kwargs): book_plots.plot_filter(filtered_data, **kwargs) book_plots.plot_measurements(measurements, label=z_label) plt.legend(loc=4) plt.title(title) plt.gca().set_xlim(left=0, right=len(measurements)) return import time if not interactive: book_plots.plot_filter(filtered_data, **kwargs) book_plots.plot_measurements(measurements, label=z_label) book_plots.show_legend() plt.title(title) plt.gca().set_xlim(left=0, right=len(measurements)) else: for i in range(2, len(measurements)): book_plots.plot_filter(filtered_data, **kwargs) book_plots.plot_measurements(measurements, label=z_label) book_plots.show_legend() plt.title(title) plt.gca().set_xlim(left=0, right=len(measurements)) plt.gca().canvas.draw() time.sleep(0.5)
def plot_g_h_results(measurements, filtered_data, title='', z_label='Measurements', **kwargs): book_plots.plot_filter(filtered_data, **kwargs) book_plots.plot_measurements(measurements, label=z_label) plt.legend(loc=4) plt.title(title) plt.gca().set_xlim(left=0,right=len(measurements)) return import time if not interactive: book_plots.plot_filter(filtered_data, **kwargs) book_plots.plot_measurements(measurements, label=z_label) book_plots.show_legend() plt.title(title) plt.gca().set_xlim(left=0,right=len(measurements)) else: for i in range(2, len(measurements)): book_plots.plot_filter(filtered_data, **kwargs) book_plots.plot_measurements(measurements, label=z_label) book_plots.show_legend() plt.title(title) plt.gca().set_xlim(left=0,right=len(measurements)) plt.gca().canvas.draw() time.sleep(0.5)
def plot_track_and_residuals(dt, xs, z_xs, res): """ plots track and measurement on the left, and the residual of the filter on the right. Helps to visualize the performance of an adaptive filter. """ assert np.isscalar(dt) t = np.arange(0, len(xs) * dt, dt) plt.subplot(121) if z_xs is not None: bp.plot_measurements(t, z_xs, label='z') bp.plot_filter(t, xs) plt.legend(loc=2) plt.xlabel('time (sec)') plt.ylabel('X') plt.title('estimates vs measurements') plt.subplot(122) # plot twice so it has the same color as the plot to the left! plt.plot(t, res) plt.plot(t, res) plt.xlabel('time (sec)') plt.ylabel('residual') plt.title('residuals') plt.show()
def plot_track_and_residuals(dt, xs, z_xs, res): """ plots track and measurement on the left, and the residual of the filter on the right. Helps to visualize the performance of an adaptive filter. """ assert np.isscalar(dt) t = np.arange(0, len(xs)*dt, dt) plt.subplot(121) if z_xs is not None: bp.plot_measurements(t, z_xs, label='z') bp.plot_filter(t, xs) plt.legend(loc=2) plt.xlabel('time (sec)') plt.ylabel('X') plt.title('estimates vs measurements') plt.subplot(122) # plot twice so it has the same color as the plot to the left! plt.plot(t, res) plt.plot(t, res) plt.xlabel('time (sec)') plt.ylabel('residual') plt.title('residuals') 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) 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_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_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_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()
x = gaussian(0., 1000.) # initial state process_model = gaussian(1., process_var) N = 12 zs = distancia(distance_std, N) ps = [] estimates = [] priors = np.zeros((N, 2)) for i, z in enumerate(zs): prior = predict(x, process_model) priors[i] = prior x = update(prior, gaussian(z, distance_std**2)) # save for latter plotting estimates.append(x.mean) ps.append(x.var) # plot the filter output and the variance book_plots.plot_measurements(zs) book_plots.plot_filter(estimates, var=np.array(ps)) book_plots.plot_predictions(priors[:, 0]) book_plots.show_legend() book_plots.set_labels(x='Tempo (s)', y='Posições') plt.show() plt.figure() plt.plot(ps) plt.title('Variância') print('Variance converges to {:.3f}'.format(ps[-1]))
x0 = 5 dt = 1 h = 0.01 #re-seed the generation np.random.seed(100) vector = gen_data(x0=x0, dx=dx, count=count, noise_factor=noise_factor) data = range(3 * len(vector)) data = np.reshape(data, (3, len(vector))) for i in range(len(g)): data[i] = filter(data=vector, x0=0., dx=5., dt=1., g=g[i], h=0.01) with book_plots.figsize(y=4): book_plots.plot_measurements(vector, color='k') book_plots.plot_filter(data[0], label='g=0.1', marker='s', c='C0') book_plots.plot_filter(data[1], label='g=0.4', marker='v', c='C1') book_plots.plot_filter(data[2], label='g=0.8', c='C2') plt.legend(loc=4) book_plots.set_limits([20, 40], [50, 250]) plt.grid() plt.show() zs = [5, 6, 7, 8, 9, 10, 11, 12, 13, 14] for i in range(50): zs.append(14) data1 = filter(data=zs, x0=4., dx=1., dt=1., g=0.1, h=0.01) data2 = filter(data=zs, x0=4., dx=1., dt=1., g=0.5, h=0.01) data3 = filter(data=zs, x0=4., dx=1., dt=1., g=0.9, h=0.01) with book_plots.figsize(y=4):