def resample(self, weights):
# print "-------------"
# Normalize weights
weights /= weights.sum()
# print "weights"
# print weights
N_eff = self.pc
if weights.sum() == 0.:
rospy.loginfo("All weights zero!")
else:
N_eff = 1 / np.sum(np.square(weights))
# print "N_eff ", N_eff
print "Missed meas ", self.miss_meas
print "Neff ", N_eff
if self.miss_meas < self.pc / 2.:
self.stats.publish(np.float32(N_eff))
# Resampling?
if N_eff < self.pc / 2. and self.miss_meas < self.pc / 2.:
indices = residual_resample(weights)
print "Resampling "
print indices
keep = list(set(indices))
lost = [i for i in range(self.pc) if i not in keep]
dupes = indices[:].tolist()
for i in keep:
dupes.remove(i)
self.reassign_poses(lost, dupes)
# Add noise to particles
for i in range(self.pc):
self.particles[i].add_noise(self.res_noise_cov)
def resample(self, weights):
# Normalize weights
weights /= weights.sum()
N_eff = self.pc
if weights.sum() == 0.:
rospy.loginfo("All weights zero!")
else:
N_eff = 1 / np.sum(np.square(weights))
self.n_eff_mask.pop(0)
self.n_eff_mask.append(N_eff)
self.n_eff_filt = self.moving_average(self.n_eff_mask, 3)
# print ("N_eff ", N_eff)
# print('n_eff_filt ', self.n_eff_filt)
# print ("Missed meas ", self.miss_meas)
# Resampling?
# if self.n_eff_filt < self.pc/2. and self.miss_meas <= self.pc/2.:
print('n_eff ', N_eff)
print("Weights ")
print(weights)
if N_eff < self.pc / 2. and self.miss_meas <= self.pc / 2.:
self.time2resample = False
self.ctr = 0
rospy.loginfo('resampling')
print("Missed meas ", self.miss_meas)
indices = residual_resample(weights)
keep = list(set(indices))
lost = [i for i in range(self.pc) if i not in keep]
dupes = indices[:].tolist()
for i in keep:
dupes.remove(i)
self.reassign_poses(lost, dupes)
def pf_step_negbin(y, x, params, W, resample=True):
"""
One step (measurement) of the particle filter, NegBin obs. model
(Resample)
Propagate the particles using the prior model,
Update weights
Remove the first elements of baselines
"""
N = W.shape[0]
if resample:
ind = resampling.residual_resample(W)
x = x[ind]
params['base'] = params['base'][:, ind]
params['sqrtQ_x'] = params['sqrtQ_x'][ind]
params['A_x'] = params['A_x'][ind]
params['omega'] = params['omega'][ind]
W = np.repeat(1 / N, N)
x = np.random.normal(
params['base'][1, :] + params['A_x'] * (x - params['base'][0, :]),
params['sqrtQ_x'])
params = trim_base(params)
params, W = pf_update_negbin(y, x, params, W)
return x, params, W
def resample(self, weights):
# Normalize weights
# print (weights)
weights /= weights.sum()
N_eff = self.pc
if weights.sum() == 0.:
rospy.loginfo("All weights zero!")
else:
N_eff = 1 / np.sum(np.square(weights))
self.n_eff_mask.pop(0)
self.n_eff_mask.append(N_eff)
self.n_eff_filt = self.moving_average(self.n_eff_mask, 3)
print("N_eff ", N_eff)
print("Missed meas ", self.miss_meas)
# Resampling?
if self.n_eff_filt < self.pc / 2. and self.miss_meas < self.pc / 2.:
# if N_eff < self.pc/2. and self.miss_meas < self.pc/2.:
indices = residual_resample(weights)
keep = list(set(indices))
lost = [i for i in range(self.pc) if i not in keep]
dupes = indices[:].tolist()
for i in keep:
dupes.remove(i)
self.reassign_poses(lost, dupes)
# Add noise to particles
for i in range(self.pc):
self.particles[i].add_noise(self.res_noise_cov)
def resample(self, weights):
print "-------------"
# Normalize weights
weights /= weights.sum()
# print "Weights"
# print weights
N_eff = self.pc
if weights.sum() == 0.:
rospy.loginfo("All weights zero!")
else:
N_eff = 1 / np.sum(np.square(weights))
print "N_eff ", N_eff
# Resampling?
if N_eff < self.pc * 0.5:
indices = residual_resample(weights)
print "Indices"
print indices
keep = list(set(indices))
lost = [i for i in range(self.pc) if i not in keep]
dupes = indices[:].tolist()
for i in keep:
dupes.remove(i)
self.reassign_poses(lost, dupes)
# Add noise to particles
for i in range(self.pc):
self.particles[i].add_noise([3., 3., 0., 0., 0., 0.0])
else:
print N_eff
rospy.loginfo(
'Number of effective particles high - not resampling')