def seed(self,
seed_pose: Pose,
pos_std_dev: float = 1.,
heading_std_dev: float = 0.3) -> None:
"""
Reinitialise particles around given seed pose
Particles are initialised with standard deviations around
position and heading as specified
"""
# log seed in ground truth portion of input log
if self.log_inputs:
self.input_log.append((None, None, seed_pose))
# if particle filter is not intialised, init it
if self._pfilter is None:
self._pfilter_init()
self._pfilter.particles = independent_sample([
norm(loc=seed_pose.x, scale=pos_std_dev).rvs,
norm(loc=seed_pose.y, scale=pos_std_dev).rvs,
norm(loc=seed_pose.heading, scale=heading_std_dev).rvs,
norm(loc=0, scale=0).rvs,
])(self._n_particles)
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")
for row in data:
preds.append(row)
preds[rownum] = [float(i) for i in preds[rownum]]
rownum += 1
nppreds = np.asarray(preds)
nplabel = np.asarray(labels)
# ------------- Particle filter setup -------------
#
columns = ["x", "y", "z", "dx", "dy", "dz"]
prior_fn = independent_sample([
# Location
norm(loc=nppreds[0, 0], scale=20).rvs,
norm(loc=nppreds[0, 1], scale=20).rvs,
norm(loc=nppreds[0, 2], scale=20).rvs,
# Speed
norm(loc=0, scale=0.5).rvs,
norm(loc=0, scale=0.5).rvs,
norm(loc=0, scale=0.5).rvs,
])
def ob_fn(x):
# observation function
y = np.zeros((x.shape[0], 3))
for i, particle in enumerate(x):
y[i, 0] = norm(loc=particle[0], scale=2).rvs(1)
y[i, 1] = norm(loc=particle[1], scale=2).rvs(1)
y[i, 2] = norm(loc=particle[2], scale=2).rvs(1)
return y
return y
#%%
# names (this is just for reference for the moment!)
columns = ["x", "y", "radius", "dx", "dy"]
# prior sampling function for each variable
# (assumes x and y are coordinates in the range 0-img_size)
prior_fn = independent_sample(
[
norm(loc=img_size / 2, scale=img_size / 2).rvs,
norm(loc=img_size / 2, scale=img_size / 2).rvs,
gamma(a=1, loc=0, scale=10).rvs,
norm(loc=0, scale=0.5).rvs,
norm(loc=0, scale=0.5).rvs,
]
)
# very simple linear dynamics: x += dx
def velocity(x):
dt = 1.0
xp = (
x
@ np.array(
[
[1, 0, 0, dt, 0],
[0, 1, 0, 0, dt],
[0, 0, 1, 0, 0],