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[ ]:
-
-
-
-