particle-filter-2.py 文件源码

def move(self, turn, forward):
        if forward < 0:
            raise ValueError, 'Cant move backwards'

        self.orientation = self.orientation + turn + random.gauss(0.0, self.turn_noise)
        self.orientation %= 2*pi

        dist = forward + random.gauss(0.0, self.forward_noise)
        self.x = self.x + dist*cos(self.orientation)
        self.y = self.y - dist*sin(self.orientation)

        self.x %= world_size
        self.y %= world_size

        temp = robot()
        temp.set_noise(0.001, 0.1, 0.1)
        temp.set(self.x, self.y, self.orientation)
        temp.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
        return temp