def Measurement_Model(self, Map, heading, distances):
##################### Measurement Motion Model #####################
# Calculate distance between robot's postion and each particle's position
# Calculate weight for each particle, w_t = exp(-distance**2/(2*self.std))
# Collect all the particles' weights in a list
# For all the weights of particles, normalized them
# Update self.weights
#Important: distances should follow a c-clockwise rotation from the vehicle's nose
robotReading = distances
w_t = np.zeros((self.numParticles, 1))
for i in range(0, self.numParticles):
rM = reducedMap(Map, self.particles[i, 0], self.particles[i, 1])
current = np.array(self.particles[i, :])
particleReading = np.array(
self.readingMap(current, rM.cutMap, heading))
#w_t[i] = np.exp( - np.linalg.norm(particleReading - robotReading)**2 / (2*self.std))
w_t[i] = 1 / np.linalg.norm(particleReading - robotReading)**4
self.readings[i, :] = particleReading
# if sum(particleReading) == 0: #Directly exclude particles in walls
# w_t[i] = 0
k = sum(w_t)
if k == 0:
print('Sum of weights is null')
self.weights = (1 / self.numParticles) * np.ones(self.numParticles)
else:
self.weights = w_t / k
def genTrajectory(self, start, goal, Map, n=200, sensorLength=5):
rM = reducedMap(Map, start[0], start[1], sensorLength)
xD = start[0]
yD = start[1]
desired = start
for i in range(n):
rM.propagateMotion(Map, xD, yD)
xD, yD = self.genDesired(xD, yD, rM.cutMap)
desired = np.vstack((desired, np.array([xD, yD])))
return desired
import numpy as np
from reduced_Map import reducedMap
from Trajectory_Generator import trajectoryGen
import matplotlib
import matplotlib.cm as cm
import matplotlib.pyplot as plt
#Load Map File
MapFile = np.genfromtxt('3Colmap.csv', delimiter=',')
walls = np.array([x for x in MapFile if x[2]>0.001])
fig, ax = plt.subplots()
ax.scatter(walls[:,0], walls[:,1], label='Walls')
pos = [8.16, 3.1]
rM = reducedMap(MapFile, pos[0], pos[1], 3)
walls1 = np.array([x for x in rM.cutMap if x[2]>0.001])
goal = [-8.16, -3.1]
ax.scatter(goal[0], goal[1], label='Goal Position')
tG = trajectoryGen(goal[0], goal[1])
xD = pos[0]
yD = pos[1]
desired = np.array(pos)
actual = np.array(pos)
for i in range(100):
xA = xD #+ float(np.random.rand(1)*3) - float(np.random.rand(1)*3)
yA = yD #+ float(np.random.rand(1)*3) - float(np.random.rand(1)*3)
rM.propagateMotion(MapFile, xA, yA)
# thatX = np.argwhere(Map[:,0] == Map[higherX,0])
# higherY = np.searchsorted(Map[thatX[:,0],1], far[1]) + higherX
newCx = np.argmin(abs(xVals - far[0]))
newCy = np.argmin(abs(yVals - far[1]))
higherY = newCx * len(yVals) + newCy
#Is there a wall at this projected point?
if Map[higherY, 2] > 0.001:
readings[i] = d
break
return readings
u_t = [0, 0]
pos = [-9.1, -2.3] #[+3.3750e+00, +4.7500e+00]
sensorLength = 5
rM = reducedMap(MapFile, pos[0], pos[1], sensorLength)
print('rM.cutMap Lenght Initialized: ', len(rM.cutMap))
rM.propagateMotion(MapFile, pos[0], pos[1])
print('rM.cutMap Lenght Propagated: ', len(rM.cutMap))
print("Create Reduced Map: %s seconds" % (time.time() - start_time))
start_time = time.time()
heading = 0
distances = readingMap(pos, rM.cutMap, heading,
sensorLength) #Front, Left, Back, Right
print("Map Reading: %s seconds" % (time.time() - start_time))
start_time = time.time()