Initialize Particles

def initialize_particles(self, init_pose):
        """
        TODO
        initialize the particles randomly around the init_pose

        Use np.random.normal to normally distribute the position
        and angle based on the standard deviations self.init_pos_dev and
        self.init_ang_dev
        """

        self.particles = np.zeros((self.num_particles, 3))
        for i in range(self.num_particles):
            self.particles[i,0] = np.random.normal(init_pose[0],self.init_pos_dev)
            self.particles[i,1] = np.random.normal(init_pose[1],self.init_pos_dev)
            self.particles[i,2] = np.random.normal(init_pose[2],self.init_ang_dev)
        print self.particles


        self.publish_pose()