diff --git a/Code/alldirparticlefilt.py b/Code/alldirparticlefilt.py deleted file mode 100644 index be75817018dd56254ec2d9e123346397157202f4..0000000000000000000000000000000000000000 --- a/Code/alldirparticlefilt.py +++ /dev/null @@ -1,243 +0,0 @@ -#!/usr/bin/env python -# coding: utf-8 - -# In[ ]: - - -#new particle filter for full vision robot - - -# In[ ]: - - - - - -# In[14]: - - -import numpy as np -import math -import copy -import random - - -# In[15]: - - -gridsize = 10 - - -# In[26]: - - -class particle: - def __init__(self,x,y,phi,pmap = {}): - self.xpos = x - self.ypos = y - self.angle = phi - self.map = copy.deepcopy(pmap) - self.weight = 1 - - def incorporate_localmap(self,localmap): - #incorporating given local map into the global map - - #loop over localmap - N = len(localmap) - for i in range(N): - for j in range(N): - #only use localmap entries that are actually measured (!=0) - if localmap[i][j] != 0: - #convert local i,j to global - local_x = (i+1/2)*gridsize - N*gridsize/2 - local_y = (j+1/2)*gridsize - N*gridsize/2 - globalmap_x = self.xpos + local_x * math.cos(self.angle) - local_y * math.sin(self.angle) - globalmap_y = self.ypos + local_x * math.sin(self.angle) + local_y * math.cos(self.angle) - globalgrid = (int(math.floor(globalmap_x/gridsize)),int(math.floor(globalmap_y/gridsize))) - #update global map, if cell never observed before create entry - if globalgrid in self.map: - self.map[globalgrid] += localmap[i][j] - else: - self.map[globalgrid] = localmap[i][j] - - def map_weigh(self,localmap): - #calculate probability of localmap corresponding to global map at particle position - - #find overlapping cells calc m_bar - N = len(localmap) - overlap = [] - m_bar = 0 - for i in range(N): - for j in range(N): - local_x = (i+1/2)*gridsize - N*gridsize/2 - local_y = (j+1/2)*gridsize - N*gridsize/2 - globalmap_x = self.xpos + local_x * math.cos(self.angle) - local_y * math.sin(self.angle) - globalmap_y = self.ypos + local_x * math.sin(self.angle) + local_y * math.cos(self.angle) - globalgrid = (int(math.floor(globalmap_x/gridsize)),int(math.floor(globalmap_y/gridsize))) - if globalgrid in self.map: - m_bar += self.map[globalgrid] + localmap[i][j] - overlap.append(((i,j),globalgrid)) - #calculate correlation between the two maps (only if there is an overlap) - if len(overlap) > 0: - m_bar = m_bar/(2*len(overlap)) - - sum_1 = 0 - sum_2 = 0 - sum_3 = 0 - for ((i,j),(k,l)) in overlap: - sum_1 += (self.map[(k,l)]-m_bar)*(localmap[i][j]-m_bar) - sum_2 += (self.map[(k,l)]-m_bar)**2 - sum_3 += (localmap[i][j]-m_bar)**2 - correlation = sum_1/math.sqrt(sum_2*sum_3) - self.weight *= max(0,correlation) - - def sample_motion(self, velocity, angular_velocity): - #sample motion for a velocity motion model - #delta_t needs to be normalized to 1! - #see probabilistic robotics page 124 - - #mean values for displacement - delta_rot1 = math.atan2(new_mean[1]-self.ypos,new_mean[0]-self.xpos) - self.angle - delta_trans = np.sqrt(((self.xpos-new_mean[0])**2)+((self.ypos-new_mean[1])**2)) - if delta_trans == 0: - delta_rot1 = 0 - delta_rot2 = new_mean[2] - self.angle - delta_rot1 - if delta_rot2 > PI: - delta_rot2 -= 2*PI - #error parameters - alpha_1 = 0.0001 #influence of velocity on velocity uncertainty - alpha_2 = 0.0001 #influence of angular velocity on velocity uncertainty - alpha_3 = 0.0001 #influence of velocity on angular velocity uncertainty - alpha_4 = 0.0001 #influence of angular velocity on angular velocity uncertainty - #an additional angle will be added to account for non circular motion - alpha_5 = 0.000001 #influence of velocity on additional angle uncertainty - alpha_6 = 0.000001 #influence of angular velocity on additional angle uncertainty - - #sample with gaussians - sample_vel = random.gauss(velocity=locity, np.sqrt(alpha_1*velocity**2+alpha_2*angular_velocity**2)) - sample_angvel = random.gauss(angular_velocity, np.sqrt(alpha_3*velocity**2 + alpha_4 * angular_velocity**2)) - sample_addangle = random.gauss(0, np.sqrt(alpha_5*velocity**2+alpha_6*angular_velocity**2)) - - if angular_velocity != 0 and abs(sample_angvel) >= 0.01: - #apply motion - self.xpos += -sample_vel/sample_angvel*math.sin(self.angle) + sample_vel/sample_angvel*math.sin(self.angle*sample_angvel) - self.ypos += sample_vel/sample_angvel*math.cos(self.angle) - sample_vel/sample_angvel*math.cos(self.angle*sample_angvel) - self.angle += sample_angvel+sample_addangle - else: - #motion for very small angular velocity, to avoid division by near 0 values - self.xpos += sample_vel*math.cos(self.angle) - self.ypos += sample_vel*math.sin(self.angle) - self.angle += sample_angvel+sample_addangle - - -# In[ ]: - - - - - -# ### Particle Filter functions -# - Propagation -# - Weighing -# - Resampling - -# In[ ]: - - -#moving all particles -#change is wheel1-,wheel2-voltage time from executed movement -def move_particles(allparticles,wheelleft,wheelright,delta_t=1): - wheeldist = 18 #distance between left/right wheel in cm - speed = 10 #distance a wheel at max voltage 1 rotates in one delta_t - - #mean speed: - mean_speed = delta_t*speed*(wheelright+wheelleft)/2 - #angular velocity - alpha_speed = (wheelright-wheelleft)*delta_t*speed/wheeldist - for i in range(len(allparticles)): - allparticles[i].sample_motion(mean_speed, alpha_speed) - -#using localmap on particle by first weighing it and than incorporating it -def weigh_and_map_particles(allparticles,localmap): - for i in range(len(allparticles)): - allparticles[i].map_weigh(localmap) - allparticles[i].incorporate_localmap(localmap) - -#resampling particles in accordance to their weights -def resampling(allparticles): - new_particles = [] - #normalizing weight - normweight = 0 - for i in range(len(allparticles)): - normweight += allparticles[i] - - rand = 0 - for i in range(len(allparticles)): - #draw random between 0,1 - rand = random.random() - j = 0 - sum_weights = allparticles[0].weight/normweight - while rand>sum_weights: - j += 1 - sum_weights += allparticles[j].weight/normweight - new_particles.append(particle(allparticles[j].xpos,allparticles[j].ypos,allparticles[j].angle,allparticles[j].map))) - - - return new_particles - -#one particle filter step: -def particle_filter(allparticles,localmap,wheelleft,wheelright,delta_t=1): - #propagate - move_particles(allparticles,wheelleft,wheelright,delta_t) - #incorporate and map - weigh_and_map_particles(allparticles,localmap) - #esampled - newparticles = resampling(allparticles) - return newparticles - - -# In[ ]: - - - - - -# In[27]: - - -#a = particle(0,0,0) -#localmap = np.ones((10,10)) - - -# In[28]: - - -#a.incorporate_localmap(localmap) -#print(a.map) - - -# In[7]: - - -#a = [] -#a.append([(0,0),(1,1)]) -# -#a.append([(0,0),(1,2)]) -# -#a.append([(0,0),(4,1)]) -#print(a) -#for ((i,j),(k,l)) in a: -# print(i,j,k,l) - - -# In[3]: - - - - - -# In[ ]: - - - -