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):
