Esempio n. 1
0
class PF(object):
    # Construct an PF instance with the following set of variables
    #    numParticles:       Number of particles
    #    Alpha:              Vector of 6 noise coefficients for the motion
    #                        model (See Table 5.3 in Probabilistic Robotics)
    #    laser:              Instance of the laser class that defines
    #                        LIDAR params, observation likelihood, and utils
    #    gridmap:            An instance of the Gridmap class that specifies
    #                        an occupancy grid representation of the map
    #                        where 1: occupied and 0: free
    #    visualize:          Boolean variable indicating whether to visualize
    #                        the particle filter
    def __init__(self, numParticles, Alpha, laser, gridmap, visualize=True):
        self.numParticles = numParticles
        self.Alpha = Alpha
        self.laser = laser
        self.gridmap = gridmap
        self.visualize = visualize
        self.ntries = 10

        # particles is a numParticles x 3 array, where each column denote a particle_handle
        # weights is a numParticles x 1 array of particle weights
        self.particles = None
        self.weights = None

        if self.visualize == True:
            self.vis = Visualization()
            self.vis.drawGridmap(self.gridmap)
        else:
            self.vis = None

    # Samples the set of particles according to a uniform distribution
    # and sets the weigts to 1/numParticles. Particles in collision are rejected
    def sampleParticlesUniform(self):

        (m, n) = self.gridmap.getShape()

        self.particles = np.empty([3, self.numParticles])

        for i in range(self.numParticles):
            theta = np.random.uniform(-np.pi, np.pi)
            inCollision = True
            while inCollision:
                x = np.random.uniform(0, (n - 1) * self.gridmap.xres)
                y = np.random.uniform(0, (m - 1) * self.gridmap.yres)

                inCollision = self.gridmap.inCollision(x, y)

            self.particles[:, i] = np.array([[x, y, theta]])

        self.weights = (1. / self.numParticles) * np.ones(
            (1, self.numParticles))

    # Samples the set of particles according to a Gaussian distribution
    # Orientation are sampled from a uniform distribution
    #    (x0, y0):    Mean position
    #    sigma:       Standard deviation
    def sampleParticlesGaussian(self, x0, y0, sigma):

        (m, n) = self.gridmap.getShape()

        self.particles = np.empty([3, self.numParticles])

        for i in range(self.numParticles):
            #theta = np.random.uniform(-np.pi,np.pi)
            inCollision = True
            while inCollision:
                x = np.random.normal(x0, sigma)
                y = np.random.normal(y0, sigma)
                theta = np.random.uniform(-np.pi, np.pi)

                inCollision = self.gridmap.inCollision(x, y)

            self.particles[:, i] = np.array([[x, y, theta]])

        self.weights = (1. / self.numParticles) * np.ones(
            (1, self.numParticles))

    # Returns desired particle (3 x 1 array) and weight
    def getParticle(self, k):

        if k < self.particles.shape[1]:
            return (self.particles[:, k], self.weights[:, k])
        else:
            print 'getParticle: Request for k=%d exceeds number of particles (%d)' % (
                k, self.particles.shape[1])
            return (None, None)

    # Return an array of normalized weights. Does not normalize the weights
    # maintained in the PF instance
    #
    # Returns:
    #   weights:   Array of normalized weights
    def getNormalizedWeights(self):

        return self.weights / np.sum(self.weights)

    # Returns the particle filter mean
    def getMean(self):

        weights = self.getNormalizedWeights()
        return np.sum(np.tile(weights,
                              (self.particles.shape[0], 1)) * self.particles,
                      axis=1)

    # Visualize filter strategies
    #   ranges:  Array of LIDAR ranges
    #   deltat:  Step size
    #   XGT:     Array with ground-truth pose
    def render(self, ranges, deltat, XGT):
        self.vis.drawParticles(self.particles, self.weights)
        if XGT is not None:
            self.vis.drawLidar(ranges, self.laser.Angles, XGT[0], XGT[1],
                               XGT[2])
            self.vis.drawGroundTruthPose(XGT[0], XGT[1], XGT[2])
        mean = self.getMean()
        self.vis.drawMeanPose(mean[0], mean[1], mean[2])
        plt.pause(deltat)

    # Sample a new pose from an initial pose (x, y, theta)
    # with inputs v (forward velocity) and w (angular velocity)
    # for deltat seconds
    #
    # This model corresponds to that in Table 5.3 in Probabilistic Robotics
    #
    # Returns:
    #   (xs, ys, thetas):   Position and heading for sample
    def sampleMotion(self, x, y, theta, u1, u2, deltat):

        # Your code goes here: Implement the algorithm given in Table 5.3
        # Note that the "sample" function in the text assumes zero-mean
        # Gaussian noise. You can use the NumPy random.normal() function
        # Be sure to reject samples that are in collision
        # (see Gridmap.inCollision), and to unwrap orientation so that it
        # it is between -pi and pi.

        abs_v = np.abs(u1)  # v
        abs_omega = np.abs(u2)  # omega

        theta = np.radians(theta)

        vt1_q = iter(
            np.random.normal(0.0,
                             np.sqrt(self.alpha1 * u1**2 +
                                     self.alpha2 * u2**2),
                             size=self.ntries))
        vt2_q = iter(
            np.random.normal(0.0,
                             np.sqrt(self.alpha3 * u1**2 +
                                     self.alpha4 * u2**2),
                             size=self.ntries))
        gammat_q = iter(
            np.random.normal(0.0,
                             np.sqrt(self.alpha5 * u1**2 +
                                     self.alpha6 * u2**2),
                             size=self.ntries))

        vhat_q = iter(
            np.random.normal(0.0,
                             np.sqrt(self.alpha1 * abs_v +
                                     self.alpha2 * abs_omega),
                             size=self.ntries))

        omegahat_q = iter(
            np.random.normal(0.0,
                             np.sqrt(self.alpha3 * abs_v +
                                     self.alpha4 * abs_omega),
                             size=self.ntries))

        gammahat_q = iter(
            np.random.normal(0.0,
                             np.sqrt(self.alpha5 * abs_v +
                                     self.alpha6 * abs_omega),
                             size=self.ntries))

        in_collision = True

        for k in range(
                self.ntries
        ):  # attempt a given number of times to find noncollision

            #vt1 = np.random.normal(0, self.alpha1 * u1**2 + self.alpha2 * u2**2)
            #vt2 = np.random.normal(0, self.alpha3 * u1**2 + self.alpha4 * u2**2)
            #gammat = np.random.normal(0, self.alpha5 * u1**2 + self.alpha6 * u2**2)

            vt1 = next(vt1_q)
            vt2 = next(vt2_q)
            gammat = next(gammat_q)

            ubart1 = u1 + vt1
            vbart2 = u2 + vt2

            xt = x + ubart1 / vbart2 * (np.sin(theta + vbart2 * deltat) -
                                        np.sin(theta))
            yt = y + ubart1 / vbart2 * (np.cos(theta) -
                                        np.cos(theta + vbart2 * deltat))
            thetat = theta + vbart2 * deltat + gammat * deltat

            # compute vhat

            #vhat = u1  + np.random.normal(0, self.alpha1 * abs_v + self.alpha2 * abs_omega)
            vhat = u1 + next(vhat_q)
            # compute omegahat

            omegahat = u2 + next(omegahat_q)

            #omegahat = u2 + np.random.normal(0, self.alpha3 * abs_v + self.alpha4 * abs_omega)

            # compute xprime

            xprime = (xt - (vhat / omegahat) * np.sin(theta) +
                      (vhat / omegahat) * np.sin(theta + omegahat * deltat))

            # compute yprime

            yprime = (yt + (vhat / omegahat) * np.cos(theta) +
                      (vhat / omegahat) * np.cos(theta + omegahat * deltat))

            # compute gammahat and thetaprime

            thetaprime = theta + (omegahat * deltat) + (next(gammahat_q) *
                                                        deltat)

            if not self.gridmap.inCollision(xprime, yprime):
                in_collision = False
                break

        if in_collision:  # if still in collision, turn around
            return np.array([x, y, np.degrees(thetaprime) + 180 % 360]).T
        else:
            return np.array([xprime, yprime, np.degrees(thetaprime)]).T

    @property
    def alpha1(self):
        return self.Alpha[0]

    @property
    def alpha2(self):
        return self.Alpha[1]

    @property
    def alpha3(self):
        return self.Alpha[2]

    @property
    def alpha4(self):
        return self.Alpha[3]

    @property
    def alpha5(self):
        return self.Alpha[4]

    @property
    def alpha6(self):
        return self.Alpha[5]

    # Function that performs resampling with replacement
    def resample(self):

        # resample self.numParticles particles according to weights
        particle_keys = np.arange(self.particles.shape[1])
        sample_size = int(self.numParticles * 0.95)
        selected_particles = np.random.choice(a=particle_keys,
                                              size=sample_size,
                                              p=self.weights.ravel())
        # add in some particles at random to avoid particle depletion
        selected_particles = np.concatenate([
            selected_particles,
            np.random.choice(a=particle_keys,
                             size=self.numParticles - sample_size)
        ])
        self.particles = self.particles[:, selected_particles]

    # Perform the prediction step
    def prediction(self, u, deltat):
        u1, u2 = u
        new_particles = np.zeros(self.particles.shape)
        for k in range(self.numParticles):
            part, wts = self.getParticle(k)
            x, y, theta = part
            new_particles[:, k] = self.sampleMotion(x, y, theta, u1, u2,
                                                    deltat)
        self.particles = new_particles

    # Perform the measurement update step
    #   Ranges:   Array of ranges (Laser.Angles provides bearings)
    def update(self, Ranges):
        # reweight particles according to obs likelihood
        self.weights = self.getNormalizedWeights()
        for particle_k in range(self.numParticles):
            # get scan likelihood based on estimated pose
            particle_pose, particle_wt = self.getParticle(particle_k)
            scan_likelihood = self.laser.scanProbability(
                Ranges, particle_pose, self.gridmap)
            self.weights[:, particle_k] = particle_wt * scan_likelihood
        self.weights = self.getNormalizedWeights()

    @property
    def effective_particles(self):
        return 1.0 / (self.weights**2).sum()  # per slides

    # Runs the particle filter algorithm
    #   U:        Array of control inputs, one column per time step
    #   Ranges:   Array of LIDAR ranges for each time step
    #             The corresponding bearings are defined in Laser.angles
    #   deltat:   Number of seconds per time step
    #   X0:       Array indicating the initial pose (may be None)
    #   XGT:      Array of ground-truth poses (may be None)
    #   filename: Name of file for plot
    def run(self, U, Ranges, deltat, X0, XGT, filename, sampleGaussian,
            staggeroreffective):

        # Try different sampling strategies (including different values for sigma)
        if sampleGaussian and (X0 is not None):
            sigma = 0.9
            self.sampleParticlesGaussian(X0[0, 0], X0[1, 0], sigma)
        else:
            self.sampleParticlesUniform()

        # Iterate over the data, for each time step
        for k in range(U.shape[1]):
            #print "At time step ", k
            u = U[:, k]
            ranges = Ranges[:, k + 1][0]

            if self.visualize:  # make sure visualize the last
                if XGT is None:
                    self.render(ranges, deltat, None)
                else:
                    self.render(ranges, deltat, XGT[:, k])

            self.prediction(u, deltat)

            self.update(ranges)

            if staggeroreffective == 'every':
                self.resample()

            if staggeroreffective == 'stagger':
                if k % 5 == 4:  # resample every few steps
                    self.resample()

            if staggeroreffective == 'effective':
                if self.effective_particles > (2.0 * self.numParticles) / 3.0:
                    self.resample()

        plt.savefig(filename)
Esempio n. 2
0
File: PF.py Progetto: mtn/ml-toybox
class PF(object):
    # Construct an PF instance with the following set of variables
    #    numParticles:       Number of particles
    #    Alpha:              Vector of 6 noise coefficients for the motion
    #                        model (See Table 5.3 in Probabilistic Robotics)
    #    laser:              Instance of the laser class that defines
    #                        LIDAR params, observation likelihood, and utils
    #    gridmap:            An instance of the Gridmap class that specifies
    #                        an occupancy grid representation of the map
    #                        where 1: occupied and 0: free
    #    visualize:          Boolean variable indicating whether to visualize
    #                        the particle filter
    def __init__(self, numParticles, Alpha, laser, gridmap, visualize=True):
        self.numParticles = numParticles
        self.Alpha = Alpha
        self.laser = laser
        self.gridmap = gridmap
        self.visualize = visualize

        # particles is a numParticles x 3 array, where each column denote a particle_handle
        # weights is a numParticles x 1 array of particle weights
        self.particles = None
        self.weights = None

        if self.visualize == True:
            self.vis = Visualization()
            self.vis.drawGridmap(self.gridmap)
        else:
            self.vis = None

    # Samples the set of particles according to a uniform distribution
    # and sets the weigts to 1/numParticles. Particles in collision are rejected
    def sampleParticlesUniform(self):

        (m, n) = self.gridmap.getShape()

        self.particles = np.empty([3, self.numParticles])

        for i in range(self.numParticles):
            theta = np.random.uniform(-np.pi, np.pi)
            inCollision = True
            while inCollision:
                x = np.random.uniform(0, (n - 1) * self.gridmap.xres)
                y = np.random.uniform(0, (m - 1) * self.gridmap.yres)

                inCollision = self.gridmap.inCollision(x, y)

            self.particles[:, i] = np.array([[x, y, theta]])

        self.weights = (1.0 / self.numParticles) * np.ones((1, self.numParticles))

    # Samples the set of particles according to a Gaussian distribution
    # Orientation are sampled from a uniform distribution
    #    (x0, y0):    Mean position
    #    sigma:       Standard deviation
    def sampleParticlesGaussian(self, x0, y0, sigma):

        (m, n) = self.gridmap.getShape()

        self.particles = np.empty([3, self.numParticles])

        for i in range(self.numParticles):
            # theta = np.random.uniform(-np.pi,np.pi)
            inCollision = True
            while inCollision:
                x = np.random.normal(x0, sigma)
                y = np.random.normal(y0, sigma)
                theta = np.random.uniform(-np.pi, np.pi)

                inCollision = self.gridmap.inCollision(x, y)

            self.particles[:, i] = np.array([[x, y, theta]])

        self.weights = (1.0 / self.numParticles) * np.ones((1, self.numParticles))

    # Returns desired particle (3 x 1 array) and weight
    def getParticle(self, k):

        if k < self.particles.shape[1]:
            return (self.particles[:, k], self.weights[:, k])
        else:
            print(
                "getParticle: Request for k=%d exceeds number of particles (%d)"
                % (k, self.particles.shape[1])
            )
            return (None, None)

    # Return an array of normalized weights. Does not normalize the weights
    # maintained in the PF instance
    #
    # Returns:
    #   weights:   Array of normalized weights
    def getNormalizedWeights(self):

        return self.weights / np.sum(self.weights)

    # Returns the particle filter mean
    def getMean(self):

        weights = self.getNormalizedWeights()
        return np.sum(
            np.tile(weights, (self.particles.shape[0], 1)) * self.particles, axis=1
        )

    # Visualize filter strategies
    #   ranges:  Array of LIDAR ranges
    #   deltat:  Step size
    #   XGT:     Array with ground-truth pose
    def render(self, ranges, deltat, XGT):
        self.vis.drawParticles(self.particles, self.weights)
        if XGT is not None:
            self.vis.drawLidar(ranges, self.laser.Angles, XGT[0], XGT[1], XGT[2])
            self.vis.drawGroundTruthPose(XGT[0], XGT[1], XGT[2])
        mean = self.getMean()
        self.vis.drawMeanPose(mean[0], mean[1], mean[2])
        plt.pause(deltat)

    # Sample a new pose from an initial pose (x, y, theta)
    # with inputs v (forward velocity) and w (angular velocity)
    # for deltat seconds
    #
    # This model corresponds to that in Table 5.3 in Probabilistic Robotics
    #
    # Returns:
    #   (xs, ys, thetas):   Position and heading for sample
    #   (u1, u2):           Control (velocity) inputs
    #   deltat:             Time increment
    def sampleMotion(self, x, y, theta, u1, u2, deltat):

        # Your code goes here: Implement the algorithm given in Table 5.3
        # Note that the "sample" function in the text assumes zero-mean
        # Gaussian noise. You can use the NumPy random.normal() function
        # Be sure to reject samples that are in collision
        # (see Gridmap.inCollision), and to unwrap orientation so that it
        # it is between -pi and pi.
        print("Please add code")

    # Function that performs resampling with replacement
    def resample(self):

        # Your code goes here
        print("Please add code")

    # Perform the prediction step
    def prediction(self, u):

        # Your code goes here
        print("Please add code")

    # Perform the measurement update step
    #   Ranges:   Array of ranges (Laser.Angles provides bearings)
    def update(self, Ranges):

        # Your code goes here
        print("Please add code")

    # Runs the particle filter algorithm
    #   U:        Array of control inputs, one column per time step
    #   Ranges:   Array of LIDAR ranges for each time step
    #             The corresponding bearings are defined in Laser.angles
    #   deltat:   Number of seconds per time step
    #   X0:       Array indicating the initial pose (may be None)
    #   XGT:      Array of ground-truth poses (may be None)
    #   filename: Name of file for plot
    def run(self, U, Ranges, deltat, X0, XGT, filename):

        # Try different sampling strategies (including different values for sigma)
        sampleGaussian = False
        if sampleGaussian and (X0 is not None):
            sigma = 0.5
            self.sampleParticlesGaussian(X0[0, 0], X0[1, 0], sigma)
        else:
            self.sampleParticlesUniform()

        # Iterate over the data
        for k in range(U.shape[1]):
            u = U[:, k]
            ranges = Ranges[:, k][0]

            if self.visualize:
                if XGT is None:
                    self.render(ranges, deltat, None)
                else:
                    self.render(ranges, deltat, XGT[:, k])
        plt.savefig(filename)
Esempio n. 3
0
class PF(object):
    # Construct an PF instance with the following set of variables
    #    numParticles:       Number of particles
    #    Alpha:              Vector of 6 noise coefficients for the motion
    #                        model (See Table 5.3 in Probabilistic Robotics)
    #    laser:              Instance of the laser class that defines
    #                        LIDAR params, observation likelihood, and utils
    #    gridmap:            An instance of the Gridmap class that specifies
    #                        an occupancy grid representation of the map
    #                        where 1: occupied and 0: free
    #    visualize:          Boolean variable indicating whether to visualize
    #                        the particle filter
    def __init__(self, numParticles, Alpha, laser, gridmap, visualize=True):
        self.numParticles = numParticles
        self.Alpha = Alpha
        self.laser = laser
        self.gridmap = gridmap
        self.visualize = visualize

        # particles is a numParticles x 3 array, where each column denote a particle_handle
        # weights is a numParticles x 1 array of particle weights
        self.particles = None
        self.weights = None

        if self.visualize == True:
            self.vis = Visualization()
            self.vis.drawGridmap(self.gridmap)
        else:
            self.vis = None

    # Samples the set of particles according to a uniform distribution
    # and sets the weigts to 1/numParticles. Particles in collision are rejected
    def sampleParticlesUniform(self):

        (m, n) = self.gridmap.getShape()

        self.particles = np.empty([3, self.numParticles])

        for i in range(self.numParticles):
            theta = np.random.uniform(-np.pi, np.pi)
            inCollision = True
            while inCollision:
                x = np.random.uniform(0, (n - 1) * self.gridmap.xres)
                y = np.random.uniform(0, (m - 1) * self.gridmap.yres)

                inCollision = self.gridmap.inCollision(x, y)

            self.particles[:, i] = np.array([[x, y, theta]])

        self.weights = (1. / self.numParticles) * np.ones(
            (1, self.numParticles))

    # Samples the set of particles according to a Gaussian distribution
    # Orientation are sampled from a uniform distribution
    #    (x0, y0):    Mean position
    #    sigma:       Standard deviation
    def sampleParticlesGaussian(self, x0, y0, sigma):

        (m, n) = self.gridmap.getShape()

        self.particles = np.empty([3, self.numParticles])

        for i in range(self.numParticles):
            #theta = np.random.uniform(-np.pi,np.pi)
            inCollision = True
            while inCollision:
                x = np.random.normal(x0, sigma)
                y = np.random.normal(y0, sigma)
                theta = np.random.uniform(-np.pi, np.pi)

                inCollision = self.gridmap.inCollision(x, y)

            self.particles[:, i] = np.array([[x, y, theta]])

        self.weights = (1. / self.numParticles) * np.ones(
            (1, self.numParticles))

    # Returns desired particle (3 x 1 array) and weight
    def getParticle(self, k):

        if k < self.particles.shape[1]:
            return (self.particles[:, k], self.weights[:, k])
        else:
            print(
                'getParticle: Request for k=%d exceeds number of particles (%d)'
                % (k, self.particles.shape[1]))
            return (None, None)

    # Return an array of normalized weights. Does not normalize the weights
    # maintained in the PF instance
    #
    # Returns:
    #   weights:   Array of normalized weights
    def getNormalizedWeights(self):

        return self.weights / np.sum(self.weights)

    # Returns the particle filter mean
    def getMean(self):

        weights = self.getNormalizedWeights()
        return np.sum(np.tile(weights,
                              (self.particles.shape[0], 1)) * self.particles,
                      axis=1)

    # Visualize filter strategies
    #   ranges:  Array of LIDAR ranges
    #   deltat:  Step size
    #   XGT:     Array with ground-truth pose
    def render(self, ranges, deltat, XGT):
        self.vis.drawParticles(self.particles, self.weights)
        if XGT is not None:
            self.vis.drawLidar(ranges, self.laser.Angles, XGT[0], XGT[1],
                               XGT[2])
            self.vis.drawGroundTruthPose(XGT[0], XGT[1], XGT[2])
        mean = self.getMean()
        self.vis.drawMeanPose(mean[0], mean[1], mean[2])
        plt.pause(deltat / 10)

    # Sample a new pose from an initial pose (x, y, theta)
    # with inputs v (forward velocity) and w (angular velocity)
    # for deltat seconds
    #
    # This model corresponds to that in Table 5.3 in Probabilistic Robotics
    #
    # Returns:
    #   (xs, ys, thetas):   Position and heading for sample
    #   (u1, u2):           Control (velocity) inputs
    #   deltat:             Time increment
    def sampleMotion(self, x, y, theta, u1, u2, deltat):

        # Your code goes here: Implement the algorithm given in Table 5.3
        # Note that the "sample" function in the text assumes zero-mean
        # Gaussian noise. You can use the NumPy random.normal() function
        # Be sure to reject samples that are in collision
        # (see Gridmap.inCollision), and to unwrap orientation so that it
        # it is between -pi and pi.

        # Hint: Repeatedly calling np.random.normal() inside a for loop
        #       can consume a lot of time. You may want to consider drawing
        #       n (e.g., n=10) samples of each noise term at once
        #       (drawing n samples is faster than drawing 1 sample n times)
        #       and if none of the estimated poses are not in collision, assume
        #       that the robot doesn't move from t-1 to t.

        N = 20
        u1_samples = np.random.normal(
            0, self.Alpha[0] * u1 * u1 * 3 + self.Alpha[1] * u2 * u2 * 3,
            N * self.numParticles)
        u2_samples = np.random.normal(
            0, self.Alpha[2] * u1 * u1 * 3 + self.Alpha[3] * u2 * u2 * 3,
            N * self.numParticles)
        g_sample = np.random.normal(
            0, self.Alpha[4] * u1 * u1 * 3 + self.Alpha[5] * u2 * u2 * 3,
            self.numParticles)

        colli = True
        i = 0
        u1hat = u1 + u1_samples[i * self.numParticles:(i + 1) *
                                self.numParticles]
        u2hat = u2 + u2_samples[i * self.numParticles:(i + 1) *
                                self.numParticles]
        div = u1hat / u2hat
        new_x = x - div * np.sin(theta) + div * np.sin(theta + u2hat * deltat)
        new_y = y + div * np.cos(theta) - div * np.cos(theta + u2hat * deltat)
        new_theta = theta + (u2 + u2_samples[i * self.numParticles:(
            i + 1) * self.numParticles]) * deltat + g_sample * deltat
        colli_result = self.gridmap.inCollision(new_x, new_y)
        while colli_result.any():
            i = i + 1
            if i == N:
                break
            colli_idx = np.where(colli_result)
            temp_u1_samples = u1_samples[i * self.numParticles:(i + 1) *
                                         self.numParticles]
            temp_u2_samples = u2_samples[i * self.numParticles:(i + 1) *
                                         self.numParticles]
            temp_u1hat = u1 + temp_u1_samples[colli_idx]
            temp_u2hat = u2 + temp_u2_samples[colli_idx]
            div = temp_u1hat / temp_u2hat
            new_x[colli_idx] = x[colli_idx] - div * np.sin(
                theta[colli_idx]) + div * np.sin(theta[colli_idx] +
                                                 temp_u2hat * deltat)
            new_y[colli_idx] = y[colli_idx] + div * np.cos(
                theta[colli_idx]) - div * np.cos(theta[colli_idx] +
                                                 temp_u2hat * deltat)
            new_theta = theta + (u2 + u2_samples[i * self.numParticles:(
                i + 1) * self.numParticles]) * deltat + g_sample * deltat
            colli_result = self.gridmap.inCollision(new_x, new_y)
            colli_idx = np.where(colli_result)

        if i == N:
            print("colli")
            new_x[colli_idx] = x[colli_idx]
            new_y[colli_idx] = y[colli_idx]
            new_theta[colli_idx] = theta[
                colli_idx] + g_sample[colli_idx] * deltat - np.pi / 8
        else:
            new_theta = theta + (u2 + u2_samples[i * self.numParticles:(
                i + 1) * self.numParticles]) * deltat + g_sample * deltat

        new_theta = ((new_theta + np.pi) % (2 * np.pi)) - np.pi
        self.particles[0, :] = new_x
        self.particles[1, :] = new_y
        self.particles[2, :] = new_theta

    # Function that performs resampling with replacement
    def resample(self):

        idx = np.random.choice(self.numParticles,
                               self.numParticles,
                               p=self.getNormalizedWeights())
        self.particles = self.particles[:, idx]

        # Your code goes here
        # The np.random.choice function may be useful

    # Perform the prediction step
    def prediction(self, u, deltat):

        self.sampleMotion(self.particles[0, :], self.particles[1, :],
                          self.particles[2, :], u[0], u[1], deltat)

    # Perform the measurement update step
    #   Ranges:   Array of ranges (Laser.Angles provides bearings)
    def update(self, Ranges):

        self.weights = self.laser.scanProbability(Ranges, self.particles,
                                                  self.gridmap)

    # Runs the particle filter algorithm
    #   U:        Array of control inputs, one column per time step
    #   Ranges:   Array of LIDAR ranges for each time step
    #             The corresponding bearings are defined in Laser.angles
    #   deltat:   Number of seconds per time step
    #   X0:       Array indicating the initial pose (may be None)
    #   XGT:      Array of ground-truth poses (may be None)
    #   filename: Name of file for plot
    def run(self, U, Ranges, deltat, X0, XGT, filename):

        # Try different sampling strategies (including different values for sigma)
        sampleGaussian = True
        print(X0)
        if sampleGaussian and (X0 is not None):
            sigma = 0.5
            self.sampleParticlesGaussian(X0[0, 0], X0[1, 0], sigma)
        else:
            self.sampleParticlesUniform()

        # Iterate over the data
        for k in range(U.shape[1]):
            #for k in range(0, 2):

            u = U[:, k]
            ranges = Ranges[:, k + 1][0]

            if self.visualize:
                if XGT is None:
                    self.render(ranges, deltat, None)
                else:
                    self.render(ranges, deltat, XGT[:, k])

            # Your code goes here

            self.prediction(u, deltat)
            self.update(ranges)
            self.resample()

        plt.savefig(filename)