particle filter (ref: https://salzis.wordpress.com/2015/05/25/particle-filters-with-python/)

pf.py

import numpy as np
from scipy.stats import norm
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from matplotlib.patches import Arrow, Circle

fig = plt.figure("pf", figsize=(7., 7.))
ax = fig.gca()

landmarks = [[20., 20.], [20., 50.], [20., 80.],
             [50., 20.],             [50., 80.],
             [80., 20.], [80., 50.], [80., 80.]]

L = 100

class Robot:

    def __init__(self, x=None, y=None, yaw=None,
                 forward_noise=None, turn_noise=None, sense_noise=None):
        self.x = x or np.random.randint(0, L)
        self.y = y or np.random.randint(0, L)
        self.yaw = yaw or np.random.random() * 2. * np.pi

        self.forward_noise = forward_noise or 0.
        self.turn_noise = turn_noise or 0.
        self.sense_noise = sense_noise or 0.

    def sense(self):
        return [
            ((self.x - m[0]) ** 2 + (self.y - m[1]) ** 2) ** 0.5 +
            np.random.normal(0., self.sense_noise)
            for m in landmarks
        ]

    def move(self, turn, forward):
        # turn
        yaw = self.yaw + turn + np.random.normal(0., self.turn_noise)
        yaw %= 2 * np.pi
        # forward
        dist = forward + np.random.normal(0., self.forward_noise)
        x = (self.x + dist * np.cos(yaw)) % L
        y = (self.y + dist * np.sin(yaw)) % L
        # return new one
        return Robot(x, y, yaw, self.forward_noise, self.turn_noise, self.sense_noise)

    @staticmethod
    def gaussian_pdf(mu, sigma, x):
        return norm(mu, sigma).pdf(x)

    def measurement_prob(self, measurement):
        prob = 1.
        for i, m in enumerate(landmarks):
            dist = ((self.x - m[0]) ** 2 + (self.y - m[1]) ** 2) ** 0.5
            prob *= self.gaussian_pdf(dist, self.sense_noise, measurement[i])
        return prob


robot = Robot()
N = 100
p = [Robot(None, None, None, 0.05, 0.05, 5) for i in range(N)]

def animate(i):
    global robot, p
    # print('step:', i)

    robot = robot.move(0.1, 1)
    p = [t.move(0.1, 1) for t in p]

    real_measurement = robot.sense()
    w = [t.measurement_prob(real_measurement) for t in p]

    p2 = []
    idx = np.random.randint(0, N)
    beta = 0.
    maxw = max(w)

    for i in range(N):
        beta += np.random.random() * 2. * maxw
        while beta > w[idx]:
            beta -= w[idx]
            idx = (idx + 1) % N
        # select
        p2.append(p[idx])
    p = p2

    ax.clear()

    for m in landmarks:
        c = Circle(m, 1., facecolor='red')
        ax.add_patch(c)

    for t in p:
        a = Arrow(t.x, t.y,
                  2. * np.cos(t.yaw), 2. * np.sin(t.yaw),
                  facecolor='blue', width=2., alpha=0.5)
        ax.add_patch(a)

    ax.axis([0, L, 0, L])

ani = animation.FuncAnimation(fig, animate, interval=100)
plt.show()