def test_pf2():
    N = 1000
    sensor_std_err = .2
    landmarks = [[-1, 2], [20, 4], [-20, 6], [18, 25]]

    pf = RobotLocalizationParticleFilter(N, 20, 20, landmarks, sensor_std_err)

    xs = []
    for x in range(18):
        zs = []
        pos = (x + 1, x + 1)

        for landmark in landmarks:
            d = np.sqrt((landmark[0] - pos[0])**2 + (landmark[1] - pos[1])**2)
            zs.append(d + randn() * sensor_std_err)

        # move diagonally forward to (x+1, x+1)
        pf.predict((0.00, 1.414), (.2, .05))
        pf.update(z=zs)
        pf.resample()

        mu, var = pf.estimate()
        xs.append(mu)

    xs = np.array(xs)
    plt.plot(xs[:, 0], xs[:, 1])
    plt.show()
Esempio n. 2
0
def test_pf2():
    N = 1000
    sensor_std_err = .2
    landmarks = [[-1, 2], [20,4], [-20,6], [18,25]]

    pf = RobotLocalizationParticleFilter(N, 20, 20, landmarks, sensor_std_err)

    xs = []
    for x in range(18):
        zs = []
        pos=(x+1, x+1)

        for landmark in landmarks:
            d = np.sqrt((landmark[0]-pos[0])**2 +  (landmark[1]-pos[1])**2)
            zs.append(d + randn()*sensor_std_err)

        # move diagonally forward to (x+1, x+1)
        pf.predict((0.00, 1.414), (.2, .05))
        pf.update(z=zs)
        pf.resample()

        mu, var = pf.estimate()
        xs.append(mu)

    xs = np.array(xs)
    plt.plot(xs[:, 0], xs[:, 1])
    plt.show()
def test_pf():

    #seed(1234)
    N = 10000
    R = .2
    landmarks = [[-1, 2], [20, 4], [10, 30], [18, 25]]
    #landmarks = [[-1, 2], [2,4]]

    pf = RobotLocalizationParticleFilter(N, 20, 20, landmarks, R)

    plot_pf(pf, 20, 20, weights=False)

    dt = .01
    plt.pause(dt)

    for x in range(18):

        zs = []
        pos = (x + 3, x + 3)

        for landmark in landmarks:
            d = np.sqrt((landmark[0] - pos[0])**2 + (landmark[1] - pos[1])**2)
            zs.append(d + randn() * R)

        pf.predict((0.01, 1.414), (.2, .05))
        pf.update(z=zs)
        pf.resample()
        #print(x, np.array(list(zip(pf.particles, pf.weights))))

        mu, var = pf.estimate()
        plot_pf(pf, 20, 20, weights=False)
        plt.plot(pos[0], pos[1], marker='*', color='r', ms=10)
        plt.scatter(mu[0], mu[1], color='g', s=100)
        plt.tight_layout()
        plt.pause(dt)
Esempio n. 4
0
def test_pf():

    #seed(1234)
    N = 10000
    R = .2
    landmarks = [[-1, 2], [20,4], [10,30], [18,25]]
    #landmarks = [[-1, 2], [2,4]]

    pf = RobotLocalizationParticleFilter(N, 20, 20, landmarks, R)

    plot_pf(pf, 20, 20, weights=False)

    dt = .01
    plt.pause(dt)

    for x in range(18):

        zs = []
        pos=(x+3, x+3)

        for landmark in landmarks:
            d = np.sqrt((landmark[0]-pos[0])**2 +  (landmark[1]-pos[1])**2)
            zs.append(d + randn()*R)

        pf.predict((0.01, 1.414), (.2, .05))
        pf.update(z=zs)
        pf.resample()
        #print(x, np.array(list(zip(pf.particles, pf.weights))))

        mu, var = pf.estimate()
        plot_pf(pf, 20, 20, weights=False)
        plt.plot(pos[0], pos[1], marker='*', color='r', ms=10)
        plt.scatter(mu[0], mu[1], color='g', s=100)
        plt.tight_layout()
        plt.pause(dt)
def run_pf1(N, iters=18, sensor_std_err=.1, 
            do_plot=True, plot_particles=False,
            xlim=(0, 20), ylim=(0, 20),
            initial_x=None):
    landmarks = np.array([[-1, 2], [5, 10], [12,14], [18,21]])
    NL = len(landmarks)
    pf = RobotLocalizationParticleFilter(
        N, 20, 20, landmarks, sensor_std_err)
    
    if initial_x is not None:
        pf.create_gaussian_particles(mean=initial_x, 
                                     var=(5, 5, np.pi/4))
        
    if plot_particles:
        alpha = .20
        if N > 5000:
            alpha *= np.sqrt(5000)/np.sqrt(N)           
        plt.scatter(pf.particles[:, 0], pf.particles[:, 1], 
                    alpha=alpha, color='g')
    
    xs = []
    pos = np.array([0., 0.])
    for x in range(iters):
        pos += (1, 1) # robot position

        # distance from robot to each landmark
        zs = norm(landmarks - pos, axis=1) + randn(NL)*sensor_std_err

        # move diagonally forward to (x+1, x+1)
        pf.predict((0.00, 1.414), (.2, .05))
        pf.update(z=zs)
        pf.resample()

        mu, var = pf.estimate()
        xs.append(mu)
        if plot_particles:
            plt.scatter(pf.particles[:, 0], pf.particles[:, 1], 
                        color='k', marker=',', s=1)
        p1 = plt.scatter(pos[0], pos[1], marker='+',
                         color='k', s=180, lw=3)
        p2 = plt.scatter(mu[0], mu[1], marker='s', color='r')
    
    xs = np.array(xs)
    #plt.plot(xs[:, 0], xs[:, 1])
    plt.legend([p1, p2], ['Actual', 'PF'], loc=4, numpoints=1)
    plt.xlim(*xlim)
    plt.ylim(*ylim)
    plt.show()
    print('final position error, variance:\n\t', mu, var)