Particle Filter

#! /usr/bin/env python

# https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/12-Particle-Filters.ipynb

import numpy as np
from scipy.stats import multivariate_normal

from numpy.random import uniform
from math import sqrt, tan, cos, sin, atan2

class ParticleFilterKinect:

	def __init__(self, num_particles, initial_pos,
		fov_vertical=43.0, fov_horizontal=57.0,
		max_depth=4.0, min_depth=0.8,
		cam_transl_x=-0.125, cam_transl_y=0.0, cam_transl_z=0.25):

		self.num_particles = num_particles
		self.particles = np.zeros((num_particles, 3))
		self.weights = np.full(num_particles, 1) / num_particles
		cam_particles = np.zeros((num_particles, 3))
		robot_particles = np.zeros((num_particles, 3))

		# Generate particles on camera space
		cam_particles[:, 2] = uniform(min_depth, max_depth, size=num_particles)

		rnd = uniform(-1, 1, size=self.num_particles)
		cam_particles[:, 0] = rnd[:] * tan(fov_horizontal/2) * cam_particles[:, 2]
		rnd = uniform(-1, 1, size=self.num_particles)
		cam_particles[:, 1] = rnd[:] * tan(fov_vertical/2) * cam_particles[:, 2]

		cam_particles[:, 0] = -cam_particles[:, 0]
		cam_particles[:, 1] = -cam_particles[:, 1]

		# From camera space to robot space
		robot_particles[:, 0] = cam_particles[:, 2] + cam_transl_x
		robot_particles[:, 1] = cam_particles[:, 0] + cam_transl_y
		robot_particles[:, 2] = cam_particles[:, 1] + cam_transl_z

		# From robot space to world space
		theta = 2*atan2(initial_pos[2], initial_pos[3])

		self.particles[:, 0] = cos(theta)*robot_particles[:, 0] - sin(theta)*robot_particles[:, 1]
		self.particles[:, 1] = sin(theta)*robot_particles[:, 0] + cos(theta)*robot_particles[:, 1]
		self.particles[:, 2] = robot_particles[:, 2]

		self.particles[:, 0] = self.particles[:, 0] + initial_pos[0]
		self.particles[:, 1] = self.particles[:, 1] + initial_pos[1]

	def predict(self, last_pos, std=(0.5, 0.5, 0.5)):
		noise = uniform(-1, 1, (self.num_particles, 3))

		self.particles[:, 0] = last_pos[0] + (noise[:, 0] * std[0])
		self.particles[:, 1] = last_pos[1] + (noise[:, 1] * std[1])
		self.particles[:, 2] = last_pos[2] + (noise[:, 2] * std[2])

	def update(self, obs_conf, observations, R=.1):
		self.weights = np.zeros(self.num_particles)
		for i, observation in enumerate(observations):
			self.weights += multivariate_normal(mean=observation, cov=[[1,0,0], [0,1,0], [0,0,1]]).pdf(self.particles) * obs_conf[i]

		self.weights += 1.e-300
		self.weights /= sum(self.weights)

	def neff(self):
		return 1. / np.sum(np.square(self.weights))

	def resample(self):
		cumulative_sum = np.cumsum(self.weights)
		cumulative_sum[-1] = 1. # avoid round-off error
		indexes = np.searchsorted(cumulative_sum, random(self.num_particles))

		self.particles[:] = self.particles[indexes]
		self.weights.fill(1.0 / self.num_particles)

	def estimate(self):
		pos = self.particles
		mean = np.average(pos, weights = self.weights, axis=0)
		var = np.average((pos-mean)**2, weights = self.weights, axis=0)

		return mean, var