<jupyter_start><jupyter_text>Importing all the needed modules.<jupyter_code>#imports
import numpy as np         
from numpy import random   
import math
import copy
import matplotlib.pyplot as plt<jupyter_output><empty_output><jupyter_text>Defining a datatype to store the information of particle and robot's position.theta is the orientation.<jupyter_code>class Pose:
  def __init__(self, x=0, y=0, theta=0):
    self.x = int(x)
    self.y = int(y)
    self.theta = theta%(2*math.pi) #orientation
    self.cord = (self.x, self.y)
 
  def __repr__(self):
    return "x="+str(self.x)+", y="+str(self.y)+", theta="+str(self.theta)<jupyter_output><empty_output><jupyter_text>Defining another object to handle noise.The mothod addNoise will add noise to the sensor realings. This is used to mimic real world sensor data that will have noise in it.<jupyter_code>class Noise:
  def __init__(self,
               linearNoise,
               angularNoise):
    self.linearNoise = linearNoise    # noise in distance reading
    self.angularNoise = angularNoise  # noise in angle reading
    
  def addError(self, view_points):
    res = []
    #angle information eliminated for simplified testing
    for p in view_points:
        #angle, distance = p
        distance = p
        #new_angle = angle + np.random.normal(0.0, self.angularNoise)
        new_distance = distance + np.random.normal(0.0, self.linearNoise)
        res.append((
            #new_angle,
             new_distance))
    return res<jupyter_output><empty_output><jupyter_text>Map object handles the operations related to the map, this same object is used to build and update the map.<jupyter_code>class Map:
  def __init__(self, dim=(20,20)):
    self.height = dim[0]
    self.width = dim[1]
    self.map = np.ones((self.height, self.width))
    self.mapPoints = []
 
  def addBlock(self, loc, color=0):
    try:
      self.map[int(loc[1])][int(loc[0])] = color
      self.mapPoints.append(loc)
    except:
      pass
 
  def draw(self):
    plt.figure()
    plt.imshow(self.map, cmap='gray')
    plt.show()
  
  def getMapPoints(self):
    return list(zip(*np.where(self.map==0)))
 
  def drawLine(self, 
               startPoint, 
               endPoint, 
               width=1, 
               show=False):
    s_y = startPoint[0]
    s_x = startPoint[1]
    e_y = endPoint[0]
    e_x = endPoint[1]
 
    if s_y > self.height or s_y < 0 or \
       s_x > self.width or s_x < 0:
      print("starting point out of boundary")
 
    if e_y > self.height or e_y < 0 or \
       e_x > self.width or e_x < 0:
      print("end point out of boundary")
    
    if s_x == e_x:
      if s_y <= e_y:
        for i in range(e_y-s_y):
          try:
            self.addBlock((s_x,s_y+i))
          except:
            pass
    
    if s_y == e_y:
      if s_x <= e_x:
        for i in range(e_x-s_x):
          try:
            self.addBlock((s_x+i,s_y))
          except:
            pass<jupyter_output><empty_output><jupyter_text>This function will generate a test map. In real world scenario this map should be provided.<jupyter_code>def defaultMap(): # build a default test map
  N_map = 50
  map = Map((N_map,N_map))
  y = 0
  map.drawLine((0,y),(9,y))
  map.drawLine((15,y),(30,y))
  map.drawLine((40,y),(50,y))
 
  y = 10
  map.drawLine((0,y),(15,y))
  map.drawLine((35,y),(40,y))
  map.drawLine((45,y),(50,y))
 
  y= 25
  map.drawLine((0,y),(9,y))
  map.drawLine((30,y),(35,y))
 
  y= 40
  map.drawLine((0,y),(9,y))
  map.drawLine((15,y),(30,y))
  map.drawLine((40,y),(50,y))
 
  ## x lines
  x= 40
  map.drawLine((x, 0),(x,12))
  map.drawLine((x,30),(x,35))
  map.drawLine((x,45),(x,50))
 
  x= 30
  map.drawLine((x, 0),(x,12))
  map.drawLine((x,15),(x,25))
  map.drawLine((x,45),(x,50))
 
  x= 10
  map.drawLine((x, 0),(x,12))
  map.drawLine((x,25),(x,35))
 
  x= 0
  map.drawLine((x, 5),(x,12))
  map.drawLine((x,30),(x,35))
  return map<jupyter_output><empty_output><jupyter_text>Visualise the map<jupyter_code>map=defaultMap()
#map.addBlock((2,5))
map.draw()<jupyter_output><empty_output><jupyter_text>Some extra utility functions that will be needed.<jupyter_code>class Utility:
  @staticmethod
  def distance(a, b):
    return np.linalg.norm(np.array([a.x, a.y]) - np.array([b.x,b.y]))
  
  @staticmethod
  def Gaussian(mu, sigma, x):
    #sigma is the gaussian noise measurement(standard deviation)
    #sigma can't be zero, division by zero error
    rtn = np.exp(-((mu-x)**2)/(sigma**2)/2.0)/np.sqrt(2.0*np.pi*(sigma**2))
    #print(rtn)
    return rtn<jupyter_output><empty_output><jupyter_text>This object will handle all the operations related to the robot. This object is used to define the real robot and also used to give the particles same property as the robot.<jupyter_code>class Robot:
  def __init__(self, pose,
               linearNoise=5.5,
               angularNoise=0.1,
               map=map):
 
    self.Pose = pose
    self.Noise = Noise(linearNoise,
                       angularNoise)
    self.setSensorSpec()
    self.mapPoints = []
    self.setMap(map)
    
  def getPose(self):
    return self.Pose
 
  def setMap(self, map):
    self.mapPoints = map.mapPoints
 
  def setNoise(self, 
               linearNoise, 
               angularNoise,
               ):
    self.Noise = Noise(linearNoise,
                       angularNoise,
                       )
 
  def setSensorSpec(self,
                    angleSweep=math.pi/4,
                    sweepNumber=8):
    self.angleSweep = angleSweep
    self.sweepNumber = sweepNumber
 
  def angleAndDistance(self, robotLocation, pointLocation):
    rx, ry = robotLocation
    px, py = pointLocation
    dx = px - rx
    dy = py - ry
    distance = math.sqrt(dx**2 + dy**2)
    angle = math.atan2(dy, dx)
    return (angle, distance)
 
  def angleRange(self, center):
    delta = self.angleSweep
    n = self.sweepNumber
    return [(center - delta) + delta * i / n for i in range(2 * n + 1)]
 
  def closestPointInSwath(self, min_angle, max_angle, robotLocation):
    pts = []
    for p in self.mapPoints:
        angle, distance = self.angleAndDistance(robotLocation, p)
        if angle > min_angle and angle < max_angle:
            pts.append(distance)
    pts = sorted(pts)
    if len(pts) > 0:
        return pts[0]
    return 0
 
  def sense(self):
    pts = []
    angles = self.angleRange(self.Pose.theta)
    delta = self.angleSweep / self.sweepNumber
    for a in angles:
        min_a = a - delta / 2.0
        max_a = a + delta / 2.0
        distance = self.closestPointInSwath(min_a, max_a, self.Pose.cord)
        if distance is not None:
            pts.append((
                #a,
                distance))
    return self.Noise.addError(pts)
 
  def moveTo(self, pose):
    self.Pose = pose<jupyter_output><empty_output><jupyter_text>Test all the things and visualise the virtual lidar readings.<jupyter_code>r = Robot(Pose(2,     # x
               2,     # y
               0.1))  # theta
d = r.sense()         # take readings from virtual lidar
print(d)
a = [i*math.pi/4 for i in range(len(d))]
print(a)
plt.scatter(a, d)<jupyter_output>[2.833157756787559, -0.04125407095517675, 2.116993820575457, 3.900315113692513, 3.6718371470289237, 5.215191117920666, -2.2669360674843997, 10.873656063945017, 6.954831759718271, -0.1748150438984748, 21.67694534320219, 14.031424694275934, 10.471831002791937, 2.0418195171407607, 10.263272875267385, 18.67153632666973, 5.0091775052113485]
[0.0, 0.7853981633974483, 1.5707963267948966, 2.356194490192345, 3.141592653589793, 3.9269908169872414, 4.71238898038469, 5.497787143782138, 6.283185307179586, 7.0685834705770345, 7.853981633974483, 8.63937979737193, 9.42477796076938, 10.210176124166829, 10.995574287564276, 11.780972450961723, 12.566370614359172]<jupyter_text>Object to handle all the operations related to particle.<jupyter_code>class Particles:
  def __init__(self,
               num_particles,
               map,
               robot):
    self.num_particles = num_particles
    self.maxHeight = map.height
    self.maxWidth = map.width
    self.map = map
    self.robot = robot
    self.linearNoise = self.robot.Noise.linearNoise
    self.init()
    self.Location_std = 3.0
    self.Direction_std = 1.0
 
  def init(self):
    self.particles = []
    for i in range(self.num_particles):
      x = np.random.uniform(0, self.maxWidth)
      y = np.random.uniform(0, self.maxHeight)
      d = np.random.uniform(0.0, 2 * math.pi)
      particle = Robot(Pose(x,y,d), )
      self.particles.append(particle)
  
  def generateWeights(self, lidarScan):
    weights = []
    totalProb = 0
    for ptl in self.particles:
      d_p = ptl.sense()
      #print(d_p)
      weight = 1.0
      for i in range(len(lidarScan)):
        weight *= Utility.Gaussian(d_p[i], self.linearNoise, lidarScan[i])
      #print(prob)
      weights.append(weight)
      totalProb += weight
    
    #print(totalProb)
    if totalProb > 1:
      return [weight/totalProb for weight in weights]
 
    else:
      return [1/float(len(weights)) for i in weights]
 
 
 
  def Resample(self, weights):
    newParticles = []
    self.bestEst = self.particles[weights.index(max(weights))].Pose
    print(weights)
    indices = [i for i,_ in enumerate(weights)]
    for i in indices:
      selected_index = np.random.choice(indices, p=weights)
      particle = self.particles[selected_index]
      newParticles.append(particle)
    self.particles = newParticles
 
  def updateWeights(self, lidarScan):
    weights = self.generateWeights(lidarScan)
    #print(weights)
    self.Resample(weights)
 
  def updatePositions(self):
    new_particles = []
    for particle in self.particles:
      p = copy.copy(particle)
      x, y = p.Pose.cord
      x += np.random.normal(0.0, self.Location_std)
      y += np.random.normal(0.0, self.Location_std)
      p.Pose.cord = (x,y)
      p.Pose.theta += np.random.normal(0.0, self.Direction_std)
      new_particles.append(p)
    self.particles = new_particles
 
  def update(self, lidarScan):
    self.updateWeights(lidarScan)
    self.updatePositions()
 
  def draw(self):
    print('particles',len(self.particles))
    temp_map = copy.deepcopy(self.map)
    
    for p in self.particles:
      #print(p.Pose.cord)
      temp_map.addBlock(p.Pose.cord, 0.8)
    #print(self.robot.Pose.cord)
    temp_map.addBlock(self.robot.Pose.cord, 0.5)
    temp_map.draw()
 
  def APL(self):
    ps = [p.Pose.cord for p in self.particles]
    x, y = zip(*ps)
    return self.bestEst<jupyter_output><empty_output><jupyter_text>Bringing everything together.<jupyter_code>r = Robot(Pose(30,30,0), #Pose
               0.1,0.1)  #noise

_p = Particles(100,   #number of particles
               map,   #grid map
               r)     #actual robot
 
T = 10   # epochs

#motion model
#path = [(20+x,20) for x in range(10)]
 
 
for i in range(T):
  print("Time step:",i)
  _p.update(r.sense())
  #motion model
  #r.moveTo(Pose(path[i][0],path[i][1],math.pi))
  location_estimation = _p.APL()
  print("actual location", r.Pose)
  print("Best estimation",location_estimation)
  _p.draw()
r.Pose.cord
_p.draw()<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.12807395440674924, 'fraction_numerical': 0.06641536528450906, 'mean_word_length': 2.774728997289973, 'pattern_counts': {'":': 0, '<': 51, '<?xml version=': 0, '>': 51, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '11', 'dir': 'jupyter-structured-clean-dedup', 'id': '123358', 'license': 'MIT', 'n_tokens_mistral': 4580, 'n_tokens_neox': 4028, 'n_words': 860, 'path': 'MCL2D.ipynb', 'repo_name': 'DarvinX/InNav', 'size': '11142'}