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)
book_plots.show_legend()
plt.title(title)
plt.gca().set_xlim(left=0,right=len(measurements))
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)
book_plots.show_legend()
plt.title(title)
plt.gca().set_xlim(left=0, right=len(measurements))
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):
n = len(weights)
xs = list(range(n+1))
book_plots.plot_filter(xs, estimates, marker='o')
book_plots.plot_measurements(xs[1:], weights, color='k', label='Scale', lines=False)
book_plots.plot_track([0, n], [160, 160+n], c='k', label='Actual Weight')
book_plots.plot_track(xs[1:], predictions, c='r', label='Predictions', marker='v')
book_plots.show_legend()
book_plots.set_labels(x='day', y='weight (lbs)')
plt.xlim([0, n])
plt.show()
def plot_track_and_residuals(t, xs, z_xs, res):
plt.subplot(121)
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_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 fusion_test(wheel_sigma, ps_sigma, do_plot=True):
dt = 0.1
kf = KalmanFilter(dim_x=2, dim_z=2)
kf.F = array([[1., dt], [0., 1.]])
kf.H = array([[1., 0.], [1., 0.]])
kf.x = array([[0.], [1.]])
kf.Q *= array([[(dt**3)/3, (dt**2)/2],
[(dt**2)/2, dt ]]) * 0.02
kf.P *= 100
kf.R[0, 0] = wheel_sigma**2
kf.R[1, 1] = ps_sigma**2
random.seed(1123)
xs, zs, nom = [], [], []
for i in range(1, 100):
m0 = i + randn()*wheel_sigma
m1 = i + randn()*ps_sigma
z = array([[m0], [m1]])
kf.predict()
kf.update(z)
xs.append(kf.x.T[0])
zs.append(z.T[0])
nom.append(i)
xs = array(xs)
zs = array(zs)
nom = array(nom)
res = nom - xs[:, 0]
#print('fusion std: {:.3f}'.format(np.std(res)))
ts = np.arange(0.1, 10, .1)
bp.plot_measurements(ts, zs[:, 0], label='Wheel')
plt.plot(ts, zs[:, 1], ls='--', label='Pos Sensor')
bp.plot_filter(ts, xs[:, 0], label='Kalman filter')
plt.legend(loc=4)
plt.ylim(0, 100)
bp.set_labels(x='time (sec)', y='meters')
plt.show()
def plot_track(ps, zs, cov, plot_P=True, y_lim=None, title='Kalman Filter'):
count = len(zs)
actual = np.linspace(0, count - 1, count)
cov = np.asarray(cov)
std = 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)
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$")
plot_covariance(cov, (0, 0))
ax = plt.subplot(122)
ax.set_title("$\sigma^2_y$")
plot_covariance(cov, (1, 1))
plt.show()
def plot_gh_results(weights, estimates, predictions):
n = len(weights)
xs = list(range(n + 1))
book_plots.plot_filter(xs, estimates, marker='o')
book_plots.plot_measurements(xs[1:],
weights,
color='k',
label='Scale',
lines=False)
book_plots.plot_track([0, n], [160, 160 + n], c='k', label='Actual Weight')
book_plots.plot_track(xs[1:],
predictions,
c='r',
label='Predictions',
marker='v')
book_plots.show_legend()
book_plots.set_labels(x='day', y='weight (lbs)')
plt.xlim([0, n])
plt.show()
def plot_track(ps, zs, cov,
plot_P=True, y_lim=None,
title='Kalman Filter'):
count = len(zs)
actual = np.linspace(0, count - 1, count)
cov = np.asarray(cov)
std = 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)
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$")
plot_covariance(cov, (0, 0))
ax = plt.subplot(122)
ax.set_title("$\sigma^2_y$")
plot_covariance(cov, (1, 1))
plt.show()
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_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()
if noise_factor is not None:
kf.R = np.eye(kf.dim_z) * noise_factor
noise_factor = 2
dt = 0.1
figure()
# initialize filter
cvfilter= make_cv_filter(dt, noise_factor)
initialize_filter(cvfilter)
xs = pos[:, 0]
z_xs = zs[:, 0]
# plot the results
kxs, _, _, _ = cvfilter.batch_filter(z_xs)
t = np.arange(0, len(z_xs) * dt, dt)
bp.plot_track(t, xs)
bp.plot_filter(t, kxs[:, 0], label='KF')
bp.set_labels(title='Track vs KF', x='time (sec)', y='X');
plt.legend(loc=4);
figure()
# reinitialize filter
initialize_filter(cvfilter)
pos2, zs2 = generate_data(120, noise_factor)
xs2 = pos2[:, 0]
z_xs2 = zs2[:, 0]
t = np.arange(0, len(xs2) * dt, dt)
# plot the results
kxs2, _, _, _ = cvfilter.batch_filter(z_xs2)
bp.plot_track(t, xs2)
bp.plot_filter(t, kxs2[:, 0], label='KF')
plt.legend(loc=4)
if __name__=='__main__':
if False:
# fixme: vary R and Q
N = 30
sensor = PosSensor1 ((0, 0), (2, 1), 1.)
zs = np.array([np.array([sensor.read()]).T for _ in range(N)])
# run filter
robot_tracker = tracker1()
mu, cov, _, _ = robot_tracker.batch_filter(zs)
for x, P in zip(mu, cov):
# covariance of x and y
cov = np.array([[P[0, 0], P[2, 0]],
[P[0, 2], P[2, 2]]])
mean = (x[0, 0], x[2, 0])
plot_covariance_ellipse(mean, cov=cov, fc='g', alpha=0.15)
print robot_tracker.P
# plot results
zs *= .3048 # convert to meters
bp.plot_filter(mu[:, 0], mu[:, 2])
bp.plot_measurements(zs[:, 0], zs[:, 1])
plt.legend(loc=2)
plt.gca().set_aspect('equal')
plt.xlim((0, 20));
if True:
a()
