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()