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_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(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(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()
x = x0
results = []
for z in data:
x_est = x + (dx * dt)
dx = dx
if pred is not None:
pred.append(x_est)
residual = z - x_est
dx = dx + h * (residual) / dt
x = x_est + g * residual
results.append(x)
return np.array(results)
weights = shapeVar
book_plots.plot_track([0, dim], [Min, Max], label='Actual weight')
data = g_h_filter(data=weights, x0=1, dx=1, g=3. / 10, h=1. / 3, dt=1.)
#plot_g_h_results(weights, data);
#plt.show()
#print(np.amin(data))
#-----------------------further-adjustment---------------------------
# for negative conc values
CMIN = 0
i = 0
for index in range(0, dim):
if (data[i] < CMIN and i < dim):
data[i] = CMIN
# print(data[i])
i = i + 1
kf.P = np.eye(kf.dim_x) * .1
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')
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)
