def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (our solution is 22 lines of code, but don't worry if you deviate from this)
# Create dict to store new beliefs
new_beliefs = {}
# Iterate through self.particles so we only re-weight tiles in which there
# are particles present
for key in self.particles.keys():
# Convert from tile row/col to coordinates
row = key[0]
col = key[1]
x = util.colToX(col)
y = util.rowToY(row)
# Pull number of particles (proxy for the old probability)
numParticles = self.particles[key]
# Calculate distance and update probability
# as numParticles * PDF result (since numParticles are the estimate
# of the posterior distribution)
mean = math.sqrt((x-agentX)**2 + (y-agentY)**2)
stdev = Const.SONAR_STD
D_t = util.pdf(mean, stdev, observedDist)
newProb = D_t*numParticles
# Assign to new beliefs dict
new_beliefs[(row, col)] = newProb
# Clear self.particles and update with resampled set
# using weightedRandomChoice
self.particles = {}
for i in range(self.NUM_PARTICLES):
# Create new weighted-random tile location for a particle
newKey = util.weightedRandomChoice(new_beliefs)
# Add to the count of particles for that tile in self.particles
self.particles[newKey] = self.particles.get(newKey, 0) + 1
# END_YOUR_CODE
self.updateBelief()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (our solution is 12 lines of code, but don't worry if you deviate from this)
# raise Exception("Not implemented yet")
# Begin reweight
# Initialize the update particles Dict randomly
updateDict = collections.defaultdict(int)
for particle in self.particles:
# Think of the particle distribution as the unnormalized posterior probability
posterior = self.particles[particle]
(row, col) = particle
# To convert from a tile to a location
X, Y = util.colToX(col), util.rowToY(row)
# mean = ||At - Ct||, At is your car's position
# Ct represents the actual location of the single other car
mean = math.sqrt((agentX - X) ** 2 + (agentY - Y) ** 2)
# Use util.pdf(mean, std, value) to compute the probability density function (PDF)
# of a Gaussian with given mean and standard deviation, evaluated at value
condition = util.pdf(mean, Const.SONAR_STD, observedDist)
# Update P = P*P(dt|ct)
updateDict[particle] = posterior * condition
# Begin resample
# Initialize the particles randomly
self.particles = collections.defaultdict(int)
# Create |self.NUM_PARTICLES| new particles during resampling.
for i in range(self.NUM_PARTICLES):
newParticle = util.weightedRandomChoice(updateDict)
self.particles[newParticle] += 1
# END_YOUR_CODE
self.updateBelief()
def observe(
self,
agentX,
agentY,
observedDist,
):
# BEGIN_YOUR_CODE (around 15 lines of code expected)
weights = dict()
for p in self.particles:
weights[p] = util.pdf(
observedDist, Const.SONAR_STD,
math.hypot(
util.rowToY(p[0]) - agentY,
util.colToX(p[1]) - agentX)) * self.particles[p]
newParticles = collections.Counter()
for i in range(self.NUM_PARTICLES):
newParticles[util.weightedRandomChoice(weights)] += 1
self.particles = newParticles
# END_YOUR_CODE
self.updateBelief()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (our solution is 6 lines of code, but don't worry if you deviate from this)
#raise Exception("Not implemented yet")
# sum up the probability
num_of_cols = self.belief.getNumCols()
num_of_rows = self.belief.getNumRows()
column = 0
new_probs = []
while column < num_of_cols:
x = util.colToX(column)
row = 0
while row < num_of_rows:
y = util.rowToY(row)
prev_prob = self.belief.getProb(row, column)
distSQ = (agentY - y)**2 + (agentX - x)**2
accuracy = util.pdf(math.sqrt(distSQ), Const.SONAR_STD,
observedDist)
new_prob = (row, column, prev_prob * accuracy)
new_probs.append(new_prob)
row += 1
column += 1
for new_prob in new_probs:
row, column, new_prob_val = new_prob
self.belief.setProb(row, column, new_prob_val)
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (our solution is 6 lines of code, but don't worry if you deviate from this)
for i in xrange(self.belief.getNumCols()):
for j in xrange(self.belief.getNumRows()):
mean = math.sqrt(((util.rowToY(i) - agentY)**2) + ((util.colToX(j) - agentX)**2))
newP = util.pdf(mean, Const.SONAR_STD, observedDist)
self.belief.setProb(i, j, newP * self.belief.getProb(i, j))
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (our solution is 6 lines of code, but don't worry if you deviate from this)
def distance(p1, p2): return math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)
for i in range(self.belief.getNumCols()):
for j in range(self.belief.getNumRows()):
emission_probability = util.pdf(distance((util.colToX(i), util.rowToY(j)), (agentX, agentY)), Const.SONAR_STD, observedDist)
self.belief.setProb(j, i, self.belief.getProb(j, i) * emission_probability)
self.belief.normalize()
def observe(self, agentX: int, agentY: int, observedDist: float) -> None:
# BEGIN_YOUR_CODE (our solution is 6 lines of code, but don't worry if you deviate from this)
for r in range(self.belief.getNumRows()):
for c in range(self.belief.getNumCols()):
true_dist = math.sqrt(((agentX - util.colToX(c))**2) + ((agentY - util.rowToY(r))**2))
nextProb = self.belief.getProb(r, c) * util.pdf(true_dist, Const.SONAR_STD, observedDist)
self.belief.setProb(r, c, nextProb)
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (our solution is 6 lines of code, but don't worry if you deviate from this)
for row in range(self.belief.numRows):
for col in range(self.belief.numCols):
dist = math.sqrt((util.colToX(col) - agentX)**2 +
(util.rowToY(row) - agentY)**2)
prob_distr = util.pdf(dist, Const.SONAR_STD, observedDist)
self.belief.setProb(row, col,
self.belief.getProb(row, col) * prob_distr)
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
for r in range(self.belief.getNumRows()):
y = util.rowToY(r)
for c in range(self.belief.getNumCols()):
bigP = self.belief.getProb(r, c)
dist = math.sqrt(
((agentX - util.colToX(c))**2 + (agentY - y)**2))
lilP = util.pdf(observedDist, Const.SONAR_STD, dist)
self.belief.setProb(r, c, bigP * lilP)
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (our solution is 6 lines of code, but don't worry if you deviate from this)
for i in range(self.belief.getNumRows()):
for j in range(self.belief.getNumCols()):
y = util.rowToY(i)
x = util.colToX(j)
dist = math.sqrt(float(agentX - x)**2 + float(agentY - y)**2)
update = util.pdf(dist, Const.SONAR_STD, observedDist)
self.belief.setProb(i, j, self.belief.getProb(i, j) * update)
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (our solution is 6 lines of code, but don't worry if you deviate from this)
for Row in range(self.belief.getNumRows()):
for Col in range(self.belief.getNumCols()):
X, Y = util.colToX(Col), util.rowToY(Row)
Pri_Prob = self.belief.getProb(Row, Col)
Distance = math.sqrt((agentX - X)**2 + (agentY - Y)**2)
Likelihood = util.pdf(Distance, Const.SONAR_STD, observedDist)
self.belief.setProb(Row, Col, Pri_Prob * Likelihood)
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (our solution is 6 lines of code, but don't worry if you deviate from this)
for r in range(self.belief.numRows):
for c in range(self.belief.numCols):
cy = util.rowToY(r)
cx = util.colToX(c)
dist = math.sqrt(
math.pow(agentX - cx, 2) + math.pow(agentY - cy, 2))
p = util.pdf(dist, Const.SONAR_STD, observedDist)
self.belief.setProb(r, c, self.belief.getProb(r, c) * p)
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (our solution is 6 lines of code, but don't worry if you deviate from this)
for row in range(self.belief.getNumRows()):
for column in range(self.belief.getNumCols()):
P = self.belief.getProb(row, column)
X, Y = util.colToX(column), util.rowToY(row)
average = math.sqrt((agentX - X) * (agentX - X) +
(agentY - Y) * (agentY - Y))
cond = util.pdf(average, Const.SONAR_STD, observedDist)
self.belief.setProb(row, column, P * cond)
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (our solution is 6 lines of code, but don't worry if you deviate from this)
def distance(x1, y1, x2, y2):
return (float(x1 - y1)**2 + float(x2 - y2)**2)**0.5
for i in range(self.belief.getNumRows()):
for j in range(self.belief.getNumCols()):
dist = distance(agentX, util.colToX(j), agentY, util.rowToY(i))
pdf = util.pdf(dist, Const.SONAR_STD, observedDist)
self.belief.setProb(i, j, self.belief.getProb(i, j) * pdf)
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (our solution is 6 lines of code, but don't worry if you deviate from this)
# raise Exception("Not implemented yet")
for row in range(self.belief.getNumRows()):
for col in range(self.belief.getNumCols()):
y, x = util.rowToY(row), util.colToX(col)
mean = math.sqrt((x - agentX)**2 + (y - agentY)**2)
pdf = util.pdf(mean, Const.SONAR_STD, observedDist)
self.belief.setProb(row, col,
self.belief.getProb(row, col) * pdf)
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (our solution is 6 lines of code, but don't worry if you deviate from this)
for row in range(self.belief.getNumRows()):
for col in range(self.belief.getNumCols()):
X, Y = util.colToX(col), util.rowToY(row)
mean = math.sqrt((agentX - X)**2 + (agentY - Y)**2)
prior = self.belief.getProb(row, col)
likelyhood = util.pdf(mean, Const.SONAR_STD, observedDist)
posterior = prior * likelyhood
self.belief.setProb(row, col, posterior)
self.belief.normalize()
def observe(self, agentX: int, agentY: int, observedDist: float) -> None:
# BEGIN_YOUR_CODE (our solution is 10 lines of code, but don't worry if you deviate from this)
for (r, c) in self.particles:
true_dist = math.sqrt(((agentX - util.colToX(c))**2) + ((agentY - util.rowToY(r))**2))
self.particles[(r, c)] *= util.pdf(true_dist, Const.SONAR_STD, observedDist)
nextParticles = collections.defaultdict(int)
for _ in range(self.NUM_PARTICLES):
nextParticles[util.weightedRandomChoice(self.particles)] += 1
self.particles = nextParticles
# END_YOUR_CODE
self.updateBelief()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (our solution is 6 lines of code, but don't worry if you deviate from this)
for row in range(self.belief.numRows):
for col in range(self.belief.numCols):
xval=util.colToX(col)
yval=util.rowToY(row)
xdiff=(xval-agentX)
ydiff=(yval-agentY)
dist=math.sqrt(xdiff*xdiff+ydiff*ydiff)
probdist=util.pdf(dist,Const.SONAR_STD,observedDist)
self.belief.setProb(row,col,probdist*self.belief.getProb(row,col))
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (around 5 lines of code expected)
# raise Exception("Not implemented yet")
# loop over each particle and then update based on the observation
for particle in self.particles.keys():
row_,col_=particle[0],particle[1]
actural_distance = math.sqrt((agentX - util.colToX(col_)) ** 2 + (agentY - util.rowToY(row_)) ** 2)
cur = self.particles[particle]
self.particles[particle]=cur*util.pdf(mean=actural_distance, std=Const.SONAR_STD, value=observedDist)
# END_YOUR_CODE
self.resample()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (our solution is 6 lines of code, but don't worry if you deviate from this)
for row in range(self.belief.getNumRows()):
for col in range(self.belief.getNumCols()):
other_x, other_y = util.colToX(col), util.rowToY(row)
true_distance = math.sqrt((agentX - other_x)**2 +
(agentY - other_y)**2)
self.belief.setProb(
row, col,
self.belief.getProb(row, col) *
util.pdf(true_distance, Const.SONAR_STD, observedDist))
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (around 10 lines of code expected)
for row in range(self.belief.getNumRows()):
for col in range(self.belief.getNumCols()):
x = util.colToX(col)
y = util.rowToY(row)
prior = self.belief.getProb(row, col)
likelihood = util.pdf(
math.sqrt((agentX - x)**2 + (agentY - y)**2),
Const.SONAR_STD, observedDist)
posterior = prior * likelihood
self.belief.setProb(row, col, posterior)
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
import math
for row in xrange(self.belief.getNumRows()):
for col in xrange(self.belief.getNumCols()):
y = util.rowToY(row)
x = util.colToX(col)
#might want to use center of agentX, agentY grid
t = (agentX - x)*(agentX - x) + (agentY-y)*(agentY-y)
t = math.sqrt(t)
self.belief.setProb(row, col, self.belief.getProb(row, col)*util.pdf(t, Const.SONAR_STD, observedDist))
#print x, y, belief.grid[row][col]
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (our solution is 6 lines of code, but don't worry if you deviate from this)
for row in range(self.belief.getNumRows()):
for col in range(self.belief.getNumCols()):
mu = math.sqrt(
sum([(c1 - c2)**2 for (c1, c2) in zip(
[agentX, agentY],
[util.colToX(col), util.rowToY(row)])]))
self.belief.setProb(
row, col,
self.belief.getProb(row, col) *
util.pdf(mu, Const.SONAR_STD, observedDist))
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
''' your code here'''
numRows = self.belief.getNumRows()
numCols = self.belief.getNumCols()
for col in range(0, numCols):
for row in range(0, numRows):
distX = util.colToX(col) - agentX
distY = util.rowToY(row) - agentY
dist = math.sqrt(distX*distX + distY*distY) # true distance
prior = self.belief.getProb(row, col)
EbarX = util.pdf(dist, Const.SONAR_STD, observedDist)
self.belief.setProb(row, col, EbarX * prior)
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (our solution is 6 lines of code, but don't worry if you deviate from this)
# raise Exception("Not implemented yet")
nb_columns, nb_rows = self.belief.getNumCols(), self.belief.getNumRows()
for row in xrange(nb_rows):
for column in xrange(nb_columns):
prob = self.belief.getProb(row, column)
x, y = util.colToX(column), util.rowToY(row)
mean = ((agentX - x) ** 2 + (agentY - y) ** 2) ** 0.5
newProb = prob * util.pdf(mean, Const.SONAR_STD, observedDist)
self.belief.setProb(row, column, newProb)
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (around 10 lines of code expected)
# raise Exception("Not implemented yet")
for row in range(self.belief.getNumRows()):
for col in range(self.belief.getNumCols()):
posterior = self.belief.getProb(row, col)
Y = util.rowToY(row)
X = util.colToX(col)
mean = math.sqrt((agentX - X)**2 + (agentY - Y)**2)
cond = util.pdf(mean, Const.SONAR_STD, observedDist)
newPosterior = posterior * cond
self.belief.setProb(row, col, newPosterior)
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (around 10 lines of code expected)
# raise Exception("Not implemented yet")
for row in range(self.belief.getNumRows()):
for col in range(self.belief.getNumCols()):
posterior = self.belief.getProb(row,col)
Y = util.rowToY(row)
X = util.colToX(col)
mean = math.sqrt((agentX - X)**2 + (agentY - Y)**2)
cond = util.pdf(mean,Const.SONAR_STD,observedDist)
newPosterior = posterior*cond
self.belief.setProb(row,col,newPosterior)
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
''' your code here'''
NumRow = self.belief.getNumRows()
NumCol = self.belief.getNumCols()
for row in range(NumRow):
for col in range(NumCol):
mean = math.sqrt((abs(agentX - util.colToX(col)))**2 +
(abs(agentY - util.rowToY(row)))**2)
pdf = util.pdf(mean, Const.SONAR_STD, observedDist)
prob = self.belief.getProb(row,
col) # prob in that position tile
newprob = prob * pdf # prob in that tile and that tile's prob
self.belief.setProb(row, col, newprob)
self.belief.normalize()
def observe(self, agentX, agentY,
observedDist): # 观测是用来修正我们预测的概率值. 也即,根据最新的观测来修正proposal.
# BEGIN_YOUR_CODE (our solution is 6 lines of code, but don't worry if you deviate from this)
numRows = self.belief.getNumRows()
numCols = self.belief.getNumCols()
for row in range(numRows):
for col in range(numCols):
x, y = util.colToX(col), util.rowToY(row)
trueDistance = math.sqrt((x - agentX)**2 + (y - agentY)**2)
likelihood = util.pdf(
trueDistance, Const.SONAR_STD,
observedDist) # compute the likelihood p(dt|ct)
self.belief.grid[row][col] *= likelihood
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
w, newP = collections.Counter(), collections.Counter()
for p in self.particles:
dist = math.sqrt((agentX - util.colToX(p[1]))**2 +
(agentY - util.rowToY(p[0]))**2)
w[p] = self.particles[p] * util.pdf(observedDist, Const.SONAR_STD,
dist)
for n in range(self.NUM_PARTICLES):
newP[util.weightedRandomChoice(w)] += 1
self.particles = newP
# END_YOUR_CODE
self.updateBelief()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (our solution is 12 lines of code, but don't worry if you deviate from this)
particles_weight = collections.defaultdict(float)
for particle in self.particles:
x = util.colToX(particle[1])
y = util.rowToY(particle[0])
dist = ((agentX - x)**2 + (agentY - y)**2)**0.5
p = util.pdf(dist, Const.SONAR_STD, observedDist)
particles_weight[particle] = self.particles[particle] * p
##resample
self.particles = collections.defaultdict(int)
for i in range(self.NUM_PARTICLES):
self.particles[util.weightedRandomChoice(particles_weight)] += 1
# END_YOUR_CODE
self.updateBelief()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (around 10 lines of code expected)
def euclidean(x1, y1, x2, y2):
return math.sqrt((y2 - y1)**2 + (x2 - x1)**2)
# Compute the pdf off of observed Dist
# For every tile in the grid
for r in range(self.belief.getNumRows()):
for c in range(self.belief.getNumCols()):
otherX, otherY = (util.colToX(c), util.rowToY(r))
dist = euclidean(agentX, agentY, otherX, otherY)
prev = self.belief.getProb(r, c)
pdf = util.pdf(observedDist, Const.SONAR_STD, dist)
# Update the posterior probability
posterior = pdf * prev
self.belief.setProb(r, c, posterior)
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (our solution is 10 lines of code, but don't worry if you deviate from this)
proposed = collections.defaultdict(float)
for row, col in self.particles:
dist = math.sqrt((util.colToX(col) - agentX)**2 +
(util.rowToY(row) - agentY)**2)
prob_distr = util.pdf(dist, Const.SONAR_STD, observedDist)
proposed[(row, col)] = self.particles[(row, col)] * prob_distr
newParticles = collections.defaultdict(int)
for i in range(self.NUM_PARTICLES):
particle = util.weightedRandomChoice(proposed)
newParticles[particle] += 1
self.particles = newParticles
# END_YOUR_CODE
self.updateBelief()
def observe(
self,
agentX,
agentY,
observedDist,
):
# BEGIN_YOUR_CODE (around 10 lines of code expected)
for row in range(self.belief.getNumRows()):
rowY = util.rowToY(row)
for col in range(self.belief.getNumCols()):
colX = util.colToX(col)
self.belief.setProb(row, col, self.belief.getProb(row,
col) * util.pdf(observedDist,
Const.SONAR_STD, math.hypot(colX
- agentX, rowY - agentY)))
self.belief.normalize()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (around 15 lines of code expected)
def euclidean(x1, y1, x2, y2):
return math.sqrt((y2 - y1)**2 + (x2 - x1)**2)
weights = {}
for tile, occurrences in self.particles.items():
weights[tile] = 0
r, c = tile
pX, pY = (util.colToX(c), util.rowToY(r))
dist = euclidean(agentX, agentY, pX, pY)
pdf = util.pdf(observedDist, Const.SONAR_STD, dist)
weights[tile] = pdf * occurrences
newParticles = collections.Counter()
for p in range(self.NUM_PARTICLES):
tile = util.weightedRandomChoice(weights)
newParticles[tile] += 1
self.particles = newParticles
# END_YOUR_CODE
self.updateBelief()
def observe(self, agentX, agentY, observedDist):
# BEGIN_YOUR_CODE (around 15 lines of code expected)
# raise Exception("Not implemented yet")
particleDict = collections.Counter()
for particle in self.particles:
posterior = self.particles[particle]
row = particle[0]
col = particle[1]
Y = util.rowToY(row)
X = util.colToX(col)
mean = math.sqrt((agentX - X)**2 + (agentY - Y)**2)
cond = util.pdf(mean,Const.SONAR_STD,observedDist)
newPosterior = posterior*cond
particleDict[particle] = newPosterior
self.particles = collections.Counter()
for i in range(self.NUM_PARTICLES):
newParticle = util.weightedRandomChoice(particleDict)
self.particles[newParticle] += 1
# END_YOUR_CODE
self.updateBelief()
def observe(
self,
agentX,
agentY,
observedDist,
):
# BEGIN_YOUR_CODE (around 15 lines of code expected)
weights = dict()
for p in self.particles:
weights[p] = util.pdf(observedDist, Const.SONAR_STD,
math.hypot(util.rowToY(p[0])
- agentY, util.colToX(p[1])
- agentX)) * self.particles[p]
newParticles = collections.Counter()
for i in range(self.NUM_PARTICLES):
newParticles[util.weightedRandomChoice(weights)] += 1
self.particles = newParticles
# END_YOUR_CODE
self.updateBelief()