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_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_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 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_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()
plt.plot(range(len(zs)), zs)
ax_acc.set_title('Measured Acceleration')
ax_off = state_fig.add_subplot(414, sharex=ax1)
plt.plot(range(N),
np.ones(N) * motion.acc_offset,
linestyle='--',
label='Ideal')
plt.plot(range(N), xests[:, 2], label='Estimated')
ax_off.set_title('$x_2[k]$: Acceleration Offset')
plt.show()
# plot residuals and theoretical bounds to evaluate performance
res = xs[:, 0] - xests[:, 0]
plt.figure()
plt.plot(res)
# 3 std devs
bp.plot_residual_limits(variances[:, 0], 3)
bp.set_labels('Heave position residuals (3$\sigma$)', 'time (s)', 'meters')
plt.show()
# NESS calculation
true_states = np.zeros((N, 3))
true_states[:, 0:1] = xs[:, 0:1]
true_states[:, 2] = np.ones((N)) * motion.acc_offset
ness = NESS(true_states, xests, ps)
print('mean NEES: ', np.mean(ness))
print('Kalman Gains:', np.asarray(kf.K))
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) bp.set_labels(title='Effects of Acceleration',
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()
