Ejemplo n.º 1
0
    def start(self):
        plt.figure()
        # simulate robot movement
        N = 30
        sensor = PosSensor1((0, 0), (2, .2), noise_std=self.R_std)

        zs = np.array([np.array([sensor.read()]).T for _ in range(N)])
        # run filter
        robot_tracker = self.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])
            #print(mean)

            #plot_covariance_ellipse(mean, cov=cov, fc='g', std=3, alpha=0.5)

        #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.xlim((0, 70))
        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()
Ejemplo n.º 3
0
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'):

    with interactive_plot():
        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()
Ejemplo n.º 4
0
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_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()
Ejemplo n.º 6
0
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'):

    with interactive_plot():
        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()
Ejemplo n.º 8
0
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()
Ejemplo n.º 9
0
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)
    plt.legend(loc=4)
    plt.title(title)
    plt.gca().set_xlim(left=0,right=len(measurements))

    return

    import time
    if not interactive:
        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))
    else:
        for i in range(2, len(measurements)):
            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))
            plt.gca().canvas.draw()
            time.sleep(0.5)
def plot_gh_results(weights, estimates, predictions, time_step=0):

    n = len(weights)
    if time_step > 0:
        rng = range(1, n + 1)
    else:
        rng = range(n, n + 1)

    plt.xlim([-1, n + 1])
    plt.ylim([156.0, 173])
    act, = book_plots.plot_track([0, n], [160, 160 + n], c='k')
    plt.gcf().canvas.draw()

    for i in rng:
        xs = list(range(i + 1))

        #plt.cla()

        pred, = book_plots.plot_track(xs[1:],
                                      predictions[:i],
                                      c='r',
                                      marker='v')
        plt.xlim([-1, n + 1])
        plt.ylim([156.0, 173])
        plt.gcf().canvas.draw()
        time.sleep(time_step)

        scale, = book_plots.plot_measurements(xs[1:],
                                              weights[:i],
                                              color='k',
                                              lines=False)
        plt.xlim([-1, n + 1])
        plt.ylim([156.0, 173])
        plt.gcf().canvas.draw()
        time.sleep(time_step)

        book_plots.plot_filter(xs[:i + 1], estimates[:i + 1], marker='o')
        plt.xlim([-1, n + 1])
        plt.ylim([156.0, 173])
        plt.gcf().canvas.draw()
        time.sleep(time_step)

        plt.legend([act, scale, pred],
                   ['Actual Weight', 'Measurement', 'Predictions'],
                   loc=4)
    book_plots.set_labels(x='day', y='weight (lbs)')
def plot_gh_results(weights, estimates, predictions, time_step=0):

    n = len(weights)
    if time_step > 0:
        rng = range(1, n+1)
    else:
        rng = range(n, n+1)

    plt.xlim([-1, n+1])
    plt.ylim([156.0, 173])
    act, = book_plots.plot_track([0, n], [160, 160+n], c='k')
    plt.gcf().canvas.draw()

    for i in rng:
        xs = list(range(i+1))

        #plt.cla()

        pred, = book_plots.plot_track(xs[1:], predictions[:i], c='r', marker='v')
        plt.xlim([-1, n+1])
        plt.ylim([156.0, 173])
        plt.gcf().canvas.draw()
        time.sleep(time_step)

        scale, = book_plots.plot_measurements(xs[1:], weights[:i], color='k', lines=False)
        plt.xlim([-1, n+1])
        plt.ylim([156.0, 173])
        plt.gcf().canvas.draw()
        time.sleep(time_step)

        book_plots.plot_filter(xs[:i+1], estimates[:i+1], marker='o')
        plt.xlim([-1, n+1])
        plt.ylim([156.0, 173])
        plt.gcf().canvas.draw()
        time.sleep(time_step)

        plt.legend([act, scale, pred], ['Actual Weight', 'Measurement', 'Predictions'], loc=4)
    book_plots.set_labels(x='day', y='weight (lbs)')