def correct(self, landmark_db):
'''
Correct the weight for each particle using RToF signatures
'''
# Magnetometer correction
dir_dist = stats.norm(self.sf.mag_dir, self.sf.noise_mag)
self.weights = dir_dist.pdf(self.particles_dir)
# Resample
self.resample_fast()
# Signature correction
signature = self.sf.getRadioSignature()
update_pf = landmark_db.match(signature, self)
# If no signature found nothing to do
if (update_pf == None):
return
# Increment signature match counts
self.sf.sig_match_cnt += 1
# Create a GMM from the update_pf
lm_means = update_pf.particles_xy
lm_weights = update_pf.normalizeWts()
# Use the GMM to update weights
new_wts = gmm.gmm_pdf(self.particles_xy, lm_means, self.lm_covars, lm_weights, False)
# If weights are all 0 return
if not np.any(new_wts):
return
self.weights = new_wts
def correct(self, landmark_db,sf): #modified by xinlei
'''
Correct the weight for each particle using RToF signatures
'''
# Magnetometer correction
#print "correcting",sf.id,sf.mag_dir,sf.rssi
dir_dist = stats.norm(self.sf.mag_dir, self.sf.noise_mag)
self.weights = dir_dist.pdf(self.particles_dir)
# Resample
self.normalizeWts() #added by xinlei
self.certainty = self.getEntropy(self.weights)
self.resample_fast()
# Signature correction
#signature = self.sf.getRadioSignature()
signature = sf.rssi
# update_pf = landmark_db.match(signature, self)
update_pf = landmark_db.match(signature, self,sf) #modified by xinlei
# If no signature found nothing to do
if (update_pf == None):
sf.sig_xy = [-1,-1]
return False #return #modified by xinlei
# Increment signature match counts
self.sf.sig_match_cnt += 1
# Create a GMM from the update_pf
lm_means = update_pf.particles_xy
lm_weights = update_pf.normalizeWts()
# Use the GMM to update weights
new_wts = gmm.gmm_pdf(self.particles_xy, lm_means, self.lm_covars, lm_weights, False)
# If weights are all 0 return
if not np.any(new_wts):
return
self.weights = new_wts
return True #added by xinlei
def generate_data(n_samples):
mc = [0.4, 0.4, 0.2] # Mixing coefficients
centroids = [
np.array([0, 0]),
np.array([3, 3]),
np.array([0, 4])
]
ccov = [
np.array([[1, 0.4], [0.4, 1]]),
np.diag((1, 2)),
np.diag((0.4, 0.1))
]
# Generate samples from the gaussian mixture model
samples = gmm.sample_gaussian_mixture(centroids, ccov, mc, samples=n_samples)
xs, ys = samples[:, 0], samples[:, 1]
probs = np.zeros([n_samples], dtype=np.float32)
for it in range(n_samples):
input_ = np.array([xs[it], np.nan])
con_cen, con_cov, new_p_k = gmm.cond_dist(input_, centroids, ccov, mc)
prob = gmm.gmm_pdf(ys[it], con_cen, con_cov, new_p_k)
probs[it] = prob
return xs, ys, probs
def generate_pdf(self):
self.pdf = gmm.gmm_pdf(self.samples, self.mean, self.var, self.weight)
'''
# Now we will find the conditional distribution of x given y
fig2 = figure()
ax1 = subplot(111)
plot(X[:, 0], X[:, 1], ',')
y = -1.0
axhline(y)
x1plt = np.linspace(axis()[0], axis()[1], 200)
for i in range(len(cen_lst)):
text(cen_lst[i][0],
cen_lst[i][1],
str(i + 1),
horizontalalignment='center',
verticalalignment='center',
size=32,
color=(0.2, 0, 0))
ex, ey = gmm.gauss_ellipse_2d(cen_lst[i], cov_lst[i])
plot(ex, ey, 'k', linewidth=0.5)
ax2 = twinx()
(con_cen, con_cov, new_p_k) = gmm.cond_dist(np.array([np.nan, y]), \
cen_lst, cov_lst, p_k)
x2plt = gmm.gmm_pdf(c_[x1plt], con_cen, con_cov, new_p_k)
ax2.plot(x1plt,
x2plt,
'r',
linewidth=2,
label='Cond. dist. of $x_1$ given $x_2=' + str(y) + '$')
ax2.legend()
ax1.set_xlabel(r'$x_1$')
ax1.set_ylabel(r'$x_2$')
ax2.set_ylabel('Probability')
xx = np.linspace(-2, 2*np.pi+2, M)
XX = np.concatenate((np.c_[xx],np.c_[xx]*np.nan), axis=1)
sigma_lst = []
for i in range(XX.shape[0]):
y, sigma = gmm.cond_moments(XX[i,:], cen_lst, cov_lst, p_k)
XX[i, 1] = y
sigma_lst.append(sigma[0][0])
#var = gmm.predict(XX, cen_lst, cov_lst, p_k) # Predict them again
#std = [np.sqrt(v[0][0]) for v in var]
std = [np.sqrt(v) for v in sigma_lst]
plot(XX[:,0], XX[:,1], label='Predicted output', lw = 2)
plot(XX[:,0], XX[:,1] - std, 'k--', label='Std. dev.')
plot(XX[:,0], XX[:,1] + std, 'k--')
legend()
# Plot the cluster ellipses
for i in range(len(cen_lst)):
x1,x2 = gmm.gauss_ellipse_2d(cen_lst[i], cov_lst[i])
plot(x1, x2, 'k', linewidth=2)
O = 0
xxx = np.linspace(-2, 2*np.pi+2, O)
for x in xxx:
inp = np.array([x, np.nan])
cen_cond, cov_cond, mc_cond = gmm.cond_dist(inp, cen_lst, cov_lst, p_k)
y = np.linspace(-1.5, 1.5, 100)
x2plt = gmm.gmm_pdf(c_[y], cen_cond, cov_cond, mc_cond)
plot(x+x2plt*0.1, y, 'k')
# Plot the cluster ellipses
for i in range(len(cen_lst)):
x1,x2 = gmm.gauss_ellipse_2d(cen_lst[i], cov_lst[i])
plot(x1, x2, 'k', linewidth=2)
title(""); xlabel(r'$x_1$'); ylabel(r'$x_2$')
# Now we will find the conditional distribution of x given y
fig2 = figure()
ax1 = subplot(111)
plot(X[:,0], X[:,1], ',')
y = -1.0
axhline(y)
x1plt = np.linspace(axis()[0], axis()[1], 200)
for i in range(len(cen_lst)):
text(cen_lst[i][0], cen_lst[i][1], str(i+1), horizontalalignment='center',
verticalalignment='center', size=32, color=(0.2,0,0))
ex,ey = gmm.gauss_ellipse_2d(cen_lst[i], cov_lst[i])
plot(ex, ey, 'k', linewidth=0.5)
ax2 = twinx()
(con_cen, con_cov, new_p_k) = gmm.cond_dist(np.array([np.nan, y]), \
cen_lst, cov_lst, p_k)
x2plt = gmm.gmm_pdf(c_[x1plt], con_cen, con_cov, new_p_k)
ax2.plot(x1plt, x2plt,'r', linewidth=2,
label='Cond. dist. of $x_1$ given $x_2='+str(y)+'$')
ax2.legend()
ax1.set_xlabel(r'$x_1$')
ax1.set_ylabel(r'$x_2$')
ax2.set_ylabel('Probability')
show()
