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()
Example #4
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)
    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()
Example #8
0
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()
Example #11
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()
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()
Example #16
0
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()

Example #17
0
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)