def test_gaussian_noise():
np.random.seed(2012)
for shape in [10, 10], [100, 1000], [500, 50]:
val = np.random.normal(0, 10)
x = np.full(shape, val)
noisy = gaussian_noise(x, np.ones(shape[1]))
assert (np.mean(noisy) - np.mean(x))**2 < 1.0
assert (np.std(noisy) - 1.0)**2 < 0.1
noisy = gaussian_noise(x, np.full(shape[1], 10.0))
assert (np.std(noisy) - 10.0)**2 < 0.1
def initialize(self, initial_position):
# Prior sampling function for each of the state variables
# Centered around initial position estimate
self.prior_fn = independent_sample([
norm(loc=initial_position[0], scale=2).rvs,
norm(loc=initial_position[1], scale=2).rvs,
uniform(loc=initial_position[2], scale=np.pi / 2).rvs
])
self.pf_lock.acquire()
self.pf = ParticleFilter(
n_particles=self.n,
prior_fn=self.prior_fn,
observe_fn=self.observation_function,
weight_fn=self.weight_function,
dynamics_fn=self.dynamics_function,
noise_fn=lambda x: gaussian_noise(x, sigmas=[0.08, 0.08, 0.08]))
self.pf.update()
rospy.loginfo("Finished initial pf update")
self.pf_lock.release()
rospy.loginfo("Released initial lock")
[0, 0, 0, 0, 1., 0], [0, 0, 0, 0, 0, 1.]]).T)
return xp
def rbf_error(x, y, sigma=1):
# RBF kernel
d = np.sum((x - y)**2, axis=1)
return np.exp(-d / (2.0 * sigma**2))
pf = ParticleFilter(
prior_fn=prior_fn,
observe_fn=ob_fn,
n_particles=500,
dynamics_fn=process_fn,
noise_fn=lambda x: gaussian_noise(x, sigmas=[5, 5, 5, 0.05, 0.05, 0.1]),
weight_fn=lambda x, y: rbf_error(x, y, sigma=2),
resample_proportion=0.7,
column_names=columns,
)
pf.init_filter()
# ------------- Running Particle filter with animation -------------
#
# class UpdateParticle(object):
# def __init__(self, ax, preds, labels):
# self.nppreds = preds
# self.nplabel = labels
# self.ax = ax
def _pfilter_init(self):
"""
Setup the particle filter.
"""
# initialisation of particle filter implementation
# refer https://github.com/johnhw/pfilter/blob/master/README.md
columns = ["x", "y", "heading", "life_span"]
map_x_size = self.carpet_map.grid.shape[1] * self.carpet_map.cell_size
map_y_size = self.carpet_map.grid.shape[0] * self.carpet_map.cell_size
def prior_fn(n_particles: int):
"""
Sample n random particles from p(x|z)
i.e. if last color is BEIGE, return a sample
where proportion self._weight_fn_p of particles lie on
random beige cells, and proportion (1-self._weight_fn_p)
lie on other cells
"""
# create a grid of sample probablilities, equal in shape
# to the map grid, such that the sum of all cells
# which match the most recent color equals self._weight_fn_p
# and the sum of all cells = 1.
p_mat = np.zeros_like(self.carpet_map.grid, dtype=float)
if self._most_recent_color is None:
matching = np.zeros_like(self.carpet_map.grid)
else:
matching = self.carpet_map.grid == self._most_recent_color.index
num_matches = np.sum(matching)
if num_matches == 0 or num_matches == self.carpet_map.grid.size:
p_mat[:] = 1 / self.carpet_map.grid.size
else:
p_mat[matching] = self._weight_fn_p / np.sum(matching)
p_mat[~matching] = (1 - self._weight_fn_p) / np.sum(~matching)
# sample from the grid using the probabilities from above
p_mat_flat = p_mat.flatten()
selected_grid_linear_indices = np.random.choice(range(
len(p_mat_flat)),
size=n_particles,
p=p_mat_flat)
#convert linear indices back to grid indices
y_indices, x_indices = np.unravel_index(
selected_grid_linear_indices, self.carpet_map.grid.shape)
# convert sampled grid indices into x/y coordinates
# add noise to sample uniformly across selected grid cells
x_coords = (x_indices +
uniform().rvs(n_particles)) * self.carpet_map.cell_size
y_coords = (self.carpet_map.grid.shape[0] -
(y_indices + uniform().rvs(n_particles))
) * self.carpet_map.cell_size
heading = uniform(loc=0, scale=2 * np.pi).rvs(n_particles)
age = np.zeros_like(heading, dtype=float)
return np.column_stack([x_coords, y_coords, heading, age])
def observe_fn(state: np.array, **kwargs) -> np.array:
return self.carpet_map.get_color_at_coords(state[:, 0:3])
def weight_fn(hyp_observed: np.array, real_observed: np.array,
**kwargs):
"""
weight p for correct observations, 1-p for incorrect
"""
p = self._weight_fn_p
correct_observation = np.squeeze(hyp_observed) == np.squeeze(
real_observed)
weights = correct_observation * p + ~correct_observation * (1 - p)
return weights
def odom_update(x: np.array, odom: np.array) -> np.array:
poses = add_poses(x[:, 0:3], np.array([odom]))
life_span = x[:, 3]
life_span += 1
return np.column_stack((poses, life_span))
self._pfilter = ParticleFilter(
prior_fn=prior_fn,
observe_fn=observe_fn,
n_particles=self._n_particles,
dynamics_fn=odom_update,
noise_fn=lambda x, odom: gaussian_noise(
x,
sigmas=[
self._odom_pos_noise, self._odom_pos_noise, self.
_odom_heading_noise, 0
]),
weight_fn=weight_fn,
resample_proportion=self._resample_proportion,
column_names=columns)