Пример #1
0
    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 (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()
Пример #4
0
    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()
Пример #5
0
 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()
Пример #6
0
 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()
Пример #7
0
 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()
Пример #8
0
 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()
Пример #9
0
 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()
Пример #10
0
 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 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()
Пример #13
0
 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()
Пример #14
0
    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)
     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, 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()
Пример #17
0
 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()
Пример #18
0
 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()
Пример #19
0
    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()
Пример #20
0
 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()
Пример #21
0
    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()
Пример #22
0
 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()
Пример #23
0
 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()
Пример #24
0
    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()
Пример #25
0
 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()
Пример #26
0
 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()
Пример #27
0
 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()
Пример #28
0
 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()
Пример #29
0
    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()
Пример #30
0
 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()
Пример #31
0
 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()
Пример #32
0
    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)
        for tile in self.particles:
            other_x, other_y = util.colToX(tile[1]), util.rowToY(tile[0])
            true_distance = math.sqrt((agentX - other_x)**2 +
                                      (agentY - other_y)**2)
            self.particles[tile] *= util.pdf(true_distance, Const.SONAR_STD,
                                             observedDist)
        new_particles = collections.defaultdict(int)
        for i in range(self.NUM_PARTICLES):
            new_particle = util.weightedRandomChoice(self.particles)
            new_particles[new_particle] += 1
        self.particles = new_particles
        # END_YOUR_CODE

        self.updateBelief()
Пример #33
0
    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()
Пример #34
0
    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()
Пример #35
0
 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()
Пример #36
0
 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()
Пример #37
0
    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()