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_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_dog_track(xs, measurement_var, process_var):
N = len(xs)
bp.plot_track([0, N-1], [1, N])
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_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 plot_track_ellipses(N, zs, ps, cov, title):
bp.plot_measurements(range(1,N + 1), zs)
plt.plot(range(1, N + 1), ps, c='b', lw=2, label='filter')
plt.legend(loc='best')
plt.title(title)
for i,p in enumerate(cov):
plot_covariance_ellipse(
(i+1, ps[i]), cov=p, variance=4,
axis_equal=False, ec='g', alpha=0.5)
if i == len(cov)-1:
s = ('$\sigma^2_{pos} = %.2f$' % p[0,0])
plt.text (20, 5, s, fontsize=18)
s = ('$\sigma^2_{vel} = %.2f$' % p[1, 1])
plt.text (20, 0, s, fontsize=18)
plt.xlim(-10, 80)
plt.gca().set_aspect('equal')
plt.show()
def plot_track_ellipses(N, zs, ps, cov, title):
bp.plot_measurements(range(1,N + 1), zs)
plt.plot(range(1, N + 1), ps, c='b', lw=2, label='filter')
plt.legend(loc='best')
plt.title(title)
for i,p in enumerate(cov):
plot_covariance_ellipse(
(i+1, ps[i]), cov=p, variance=4,
axis_equal=False, ec='g', alpha=0.5)
if i == len(cov)-1:
s = ('$\sigma^2_{pos} = %.2f$' % p[0,0])
plt.text (20, 5, s, fontsize=18)
s = ('$\sigma^2_{vel} = %.2f$' % p[1, 1])
plt.text (20, 0, s, fontsize=18)
plt.xlim(-10, 80)
plt.gca().set_aspect('equal')
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()
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()
def a():
class ConstantVelocityObject(object):
def __init__(self, x0=0, vel=1., noise_scale=0.06):
self.x = x0
self.vel = vel
self.noise_scale = noise_scale
def update(self):
self.vel += randn() * self.noise_scale
self.x += self.vel
return (self.x, self.vel)
def sense(x, noise_scale=1.):
return x[0] + randn()*noise_scale
np.random.seed(124)
obj = ConstantVelocityObject()
xs, zs = [], []
for i in range(50):
x = obj.update()
z = sense(x)
xs.append(x)
zs.append(z)
xs = np.asarray(xs)
bp.plot_track(xs[:, 0])
bp.plot_measurements(range(len(zs)), zs)
plt.legend(loc='best');
def ZeroOrderKF(R, Q, P=20):
""" Create zero order Kalman filter.
Specify R and Q as floats."""
kf = KalmanFilter(dim_x=1, dim_z=1)
kf.x = np.array([0.])
kf.R *= R
kf.Q *= Q
kf.P *= P
kf.F = np.eye(1)
kf.H = np.eye(1)
return kf
def FirstOrderKF(R, Q, dt):
""" Create first order Kalman filter.
Specify R and Q as floats."""
kf = KalmanFilter(dim_x=2, dim_z=1)
kf.x = np.zeros(2)
kf.P *= np.array([[100, 0], [0, 1]])
kf.R *= R
kf.Q = Q_discrete_white_noise(2, dt, Q)
kf.F = np.array([[1., dt],
[0., 1]])
kf.H = np.array([[1., 0]])
return kf
def SecondOrderKF(R_std, Q, dt, P=100):
""" Create second order Kalman filter.
Specify R and Q as floats."""
kf = KalmanFilter(dim_x=3, dim_z=1)
kf.x = np.zeros(3)
kf.P[0, 0] = P
kf.P[1, 1] = 1
kf.P[2, 2] = 1
kf.R *= R_std**2
kf.Q = Q_discrete_white_noise(3, dt, Q)
kf.F = np.array([[1., dt, .5*dt*dt],
[0., 1.,
dt],
[0., 0.,
1.]])
kf.H = np.array([[1., 0., 0.]])
return kf
def simulate_system(Q, count):
obj = ConstantVelocityObject(x0=.0, vel=0.5, noise_scale=Q)
xs, zs = [], []
for i in range(count):
x = obj.update()
z = sense(x)
xs.append(x)
zs.append(z)
return np.asarray(xs), np.asarray(zs)
def filter_data(kf, zs):
xs, ps = [], []
for z in zs:
kf.predict()
kf.update(z)
xs.append(kf.x)
ps.append(kf.P.diagonal()) # just save variances
return np.asarray(xs), np.asarray(ps)
def plot_residuals(xs, filter_xs, Ps, title, y_label, stds=1):
res = xs - filter_xs
plt.plot(res)
bp.plot_residual_limits(Ps, stds)
bp.set_labels(title, 'time (sec)', y_label)
plt.show()
# 1
R, Q = 1, 0.03
xs, zs = simulate_system(Q=Q, count=50)
kf = FirstOrderKF(R, Q, dt=1)
filter_xs1, ps1 = filter_data(kf, zs)
plt.figure()
bp.plot_kf_output(xs, filter_xs1, zs)
plot_residuals(xs[:, 0], filter_xs1[:, 0], ps1[:, 0],
title='First Order Position Residuals(1$\sigma$)', y_label='meters')
plot_residuals(xs[:, 1], filter_xs1[:, 1], ps1[:, 1],
title='First Order Velocity Residuals(1$\sigma$)',
y_label='meters/sec')
kf0 = ZeroOrderKF(R, Q)
filter_xs0, ps0 = filter_data(kf0, zs)
plot_kf_output(xs, filter_xs0, zs)