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