Particle Filter 공부했다 (2)

 지금 까지는 좌표 위의 임의의 점을 로봇이라 칭하면서 이동시키는 상상의(?) 로봇 프로그래밍을 하였었다. 위 사진을 보면 알 수 있겠지만, 이번에는 보다 로봇스러운 시스템을 구축해보려고 한다. 이 로봇 모델은 자전거와 비슷한 구조를 가지고 있다. 앞바퀴만 각도를 바꿀 수 있고, 뒷바퀴는 그저 따라오는 형식이다.

 뒷바퀴 사이의 중점 좌표(X, Y)

 앞바퀴에서 뒷바퀴 사이의 거리(L)

 로봇 전체가 보고 있는 각도(Θ)

Θ를 기준으로 기울어진 앞바퀴의 각도()

 이렇게 시스템을 구축한다. 다음으로 좀 복잡한 수식이 나오므로 미리 마음의 준비를 하시고, 

 짜잔... 앞바퀴가 기울어진 상태로 앞으로만 움직인다면, 로봇은 원의 궤도를 그리게 될 것이다. 그래서 그 원의 중심을 구할 수 있고, 움직인 뒤의 로봇의 위치를 앞서 구한 원의 중심을 이용해서 나타낼 수 있다. 사실 슥슥 수식을 쓰는데 내 머리속에서는 ?만 가득했다. 그런데 사실 다음 단원에서 도움을 주고 있다. 

 앞바퀴가 기울어진 정도가 alpha-steering angle이고 이렇게 앞바퀴만 기울어진 채로 앞으로 나아가면 로봇이 원의 궤도를 그리게 될 것이라고 말했었다. 그런데 앞바퀴와 뒷바퀴 사이의 거리 L이 존재함에 따라 위와 같이 각각 바퀴가 그리는 원은 다른 지름을 가지게 될 것이다.

 뒷바퀴 중심의 좌표를 기준으로 한다고 했었다. 그래서 우리가 구해야 하는 것은 작은 원의 중심이다! 이를 r이라 하자! 위 그림에서 원에 접선과 중심이 이루는 각도는 항상 90도임을 이용해서 r을 구하는 것을 보여주고 있다. r을 구하는 것은 이제 해결이 되었고, 다음으로 이 원 중심의 좌표를 구해보자!!

우리는 지금 뒷바퀴 중심의 좌표와 원의 반지름을 알고 있는 상황이다. 이를 가지고 원중심의 좌표를 구해야 한다. 

 이번에도 원의 접선과 중심이 이루는 각도는 직각이라는 것을 이용해 볼 것이다. 그럼 위와 같이 로봇이 향하고 있는 각도 Θ를 이용해서 원의 중심을 구할 수 있음을 보인다. 현재 로봇의 위치 x,y에서 원의 반지름과 삼각함수를 이용해 구하는 것이다. 

 방금 구한 원의 중심을 이용해서 로봇이 다시 이동했을 때의 새로운 위치를 표현할 수도 있다!! 우선 steering angle을 유지한 채로 움직였다는 가정 하에, 원의 중심에 대해서 움직인 각도를 beta(B)라고 하겠다. 사진에서 새로이 그려진 파란 직각 삼각형에 대해서 새로운 x, y, Θ를 정의할 수 있다. 다만 마지막에 mod 2 pi 가 되어 있는데 이는 각도가 한 바퀴를 넘었을 경우에 대한 예외처리이다. 

 그럼 다시 돌아와서 원래 식을 본다. 아하!! 이해가 되었다. 그런데 밑에 B의 절댓값이 0.001보다 작을 때~ 이렇게 나와 있다. 이는 움직인 거리가 매우 작을 때에 대해서인데, R = d/B에 의해서 beta가 너무 작아 버리면 중심이 되는 원의 반지름이 너무나 커져 버린다. 이와 동시에 작게 움직이는 경우에는 움직임의 중심을 구하는 것의 의미가 거의 없어진다. 그래서 이러한 상황에서는 그냥 아래와 같은 식을 쓰는 것이 좋다고 이야기하고 있다.

 지난 실습때 사용했던 robot()클래스에 이 수식을 사용해서 코드를 수정하라는 것이 실습이다.

    def move(self, motion): # Do not change the name of this function
 
        # ADD CODE HERE
        steering = motion[0]
        distance = motion[1]
        
        if abs(steering) > max_steering_angle:
            raise ValueError
 
        if distance < 0.0:
            raise ValueError
 
        result = robot(20)
        result.length = self.length
        result.bearing_noise = self.bearing_noise
        result.steering_noise = self.steering_noise
        result.distance_noise = self.distance_noise
 
        turn = tan(steering) * distance / self.length
 
        if abs(turn) < 0.001:
            result.x = self.x + distance * cos(self.orientation)
            result.y = self.y + distance * sin(self.orientation)
            result.orientation = (self.orientation + turn) % (2.0 * pi)
        else :
            R = distance / turn
            cx = self.x - R * sin(self.orientation)
            cy = self.y + R * cos(self.orientation)
            result.x = cx + R * sin(self.orientation + turn)
            result.y = cy - R * cos(self.orientation + turn)
            result.orientation = (self.orientation + turn) % (2. * pi)
        return result 

 뽀로롱~ 위와 같다. 사실 수식도 다 줬고, 그대로 옮기기만 하면 되는 것였는데 ㅠㅠ 나는 아직 멀었다. 실제로 output을 보고 싶으면 아래 풀 소스를 돌려 보면 된다.

# -----------------
# USER INSTRUCTIONS
#
# Write a function in the class robot called move()
#
# that takes self and a motion vector (this
# motion vector contains a steering* angle and a
# distance) as input and returns an instance of the class
# robot with the appropriate x, y, and orientation
# for the given motion.
#
# *steering is defined in the video
# which accompanies this problem.
#
# For now, please do NOT add noise to your move function.
#
# Please do not modify anything except where indicated
# below.
#
# There are test cases which you are free to use at the
# bottom. If you uncomment them for testing, make sure you
# re-comment them before you submit.
 
from math import *
import random
# --------
# 
# the "world" has 4 landmarks.
# the robot's initial coordinates are somewhere in the square
# represented by the landmarks.
#
# NOTE: Landmark coordinates are given in (y, x) form and NOT
# in the traditional (x, y) format!
 
landmarks  = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of 4 landmarks
world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel "out of bounds"
max_steering_angle = pi/4 # You don't need to use this value, but it is good to keep in mind the limitations of a real car.
 
# ------------------------------------------------
# 
# this is the robot class
#
 
class robot:
 
    # --------
 
    # init: 
    #    creates robot and initializes location/orientation 
    #
 
    def __init__(self, length = 10.0):
        self.x = random.random() * world_size # initial x position
        self.y = random.random() * world_size # initial y position
        self.orientation = random.random() * 2.0 * pi # initial orientation
        self.length = length # length of robot
        self.bearing_noise  = 0.0 # initialize bearing noise to zero
        self.steering_noise = 0.0 # initialize steering noise to zero
        self.distance_noise = 0.0 # initialize distance noise to zero
    
    def __repr__(self):
        return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))
    # --------
    # set: 
    #    sets a robot coordinate
    #
 
    def set(self, new_x, new_y, new_orientation):
 
        if new_orientation < 0 or new_orientation >= 2 * pi:
            raise ValueError
        self.x = float(new_x)
        self.y = float(new_y)
        self.orientation = float(new_orientation)
 
 
    # --------
    # set_noise: 
    #    sets the noise parameters
    #
 
    def set_noise(self, new_b_noise, new_s_noise, new_d_noise):
        # makes it possible to change the noise parameters
        # this is often useful in particle filters
        self.bearing_noise  = float(new_b_noise)
        self.steering_noise = float(new_s_noise)
        self.distance_noise = float(new_d_noise)
    
    ############# ONLY ADD/MODIFY CODE BELOW HERE ###################
 
    # --------
    # move:
    #   move along a section of a circular path according to motion
    #
    
    def move(self, motion): # Do not change the name of this function
 
        # ADD CODE HERE
        steering = motion[0]
        distance = motion[1]
        
        if abs(steering) > max_steering_angle:
            raise ValueError
 
        if distance < 0.0:
            raise ValueError
 
        result = robot(20)
        result.length = self.length
        result.bearing_noise = self.bearing_noise
        result.steering_noise = self.steering_noise
        result.distance_noise = self.distance_noise
 
        turn = tan(steering) * distance / self.length
 
        if abs(turn) < 0.001:
            result.x = self.x + distance * cos(self.orientation)
            result.y = self.y + distance * sin(self.orientation)
            result.orientation = (self.orientation + turn) % (2.0 * pi)
        else :
            R = distance / turn
            cx = self.x - R * sin(self.orientation)
            cy = self.y + R * cos(self.orientation)
            result.x = cx + R * sin(self.orientation + turn)
            result.y = cy - R * cos(self.orientation + turn)
            result.orientation = (self.orientation + turn) % (2. * pi)
        return result # make sure your move function returns an instance
                      # of the robot class with the correct coordinates.
                      
    ############## ONLY ADD/MODIFY CODE ABOVE HERE ####################
        
 
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out. Our testing program provides its own code for testing your
## move function with randomized motion data.
 
## --------
## TEST CASE:
## 
## 1) The following code should print:
##       Robot:     [x=0.0 y=0.0 orient=0.0]
##       Robot:     [x=10.0 y=0.0 orient=0.0]
##       Robot:     [x=19.861 y=1.4333 orient=0.2886]
##       Robot:     [x=39.034 y=7.1270 orient=0.2886]
##
##
##length = 20.
##bearing_noise  = 0.0
##steering_noise = 0.0
##distance_noise = 0.0
##
##myrobot = robot(length)
##myrobot.set(0.0, 0.0, 0.0)
##myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
##
##motions = [[0.0, 10.0], [pi / 6.0, 10], [0.0, 20.0]]
##
##T = len(motions)
##
##print 'Robot:    ', myrobot
##for t in range(T):
##    myrobot = myrobot.move(motions[t])
##    print 'Robot:    ', myrobot
##
##
 
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out. Our testing program provides its own code for testing your
## move function with randomized motion data.
 
    
## 2) The following code should print:
##      Robot:     [x=0.0 y=0.0 orient=0.0]
##      Robot:     [x=9.9828 y=0.5063 orient=0.1013]
##      Robot:     [x=19.863 y=2.0201 orient=0.2027]
##      Robot:     [x=29.539 y=4.5259 orient=0.3040]
##      Robot:     [x=38.913 y=7.9979 orient=0.4054]
##      Robot:     [x=47.887 y=12.400 orient=0.5067]
##      Robot:     [x=56.369 y=17.688 orient=0.6081]
##      Robot:     [x=64.273 y=23.807 orient=0.7094]
##      Robot:     [x=71.517 y=30.695 orient=0.8108]
##      Robot:     [x=78.027 y=38.280 orient=0.9121]
##      Robot:     [x=83.736 y=46.485 orient=1.0135]
##
##
length = 20.
bearing_noise  = 0.0
steering_noise = 0.0
distance_noise = 0.0
 
myrobot = robot(length)
myrobot.set(0.0, 0.0, 0.0)
myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
 
motions = [[0.2, 10.] for row in range(10)]
 
T = len(motions)
 
print('Robot:    ', myrobot)
for t in range(T):
    myrobot = myrobot.move(motions[t])
    print('Robot:    ', myrobot)
 
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out. Our testing program provides its own code for testing your
## move function with randomized motion data.

 이렇게만 봐서는 사실 결과가 어떤 의미인지 알기 힘들 것이다. 추후에 plot으로 나타내는 부분을 더 넣겠다. github에 다른 사람이 만들어 놓은 것도 있다.

 아직 끝나지 않았다!! 이번에는 각 landmark들과 로봇에 대해서 이루는 각도들을 구해야 한다!! 사실 math라이브러리에 atan2함수를 써서 쉽게 할 수가 있다. 다만, 2pi를 넘어가고 하는 예외들을 조심스럽게 처리해 주어야 한다. 

    def sense(self): #do not change the name of this function
        Z = []
 
        for i in range(len(landmarks)):
            bearing = atan2(landmarks[i][0] - self.y, landmarks[i][1] - self.x) - self.orientation
            Z.append(bearing % (2. * pi))

 다소 간단하다!! atan2라는 함수는 파이썬 뿐만 아니라 보편적인 함수이므로 알아두면 좋다. 

 그럼 마지막 실습으로 이 모델을 이용해서 Particle Filter를 돌려보자!!!

 2pi / 10 즉 36도를 돌고 20만큼 앞으로 가는 과정을 반복할 것이다. 그리고 motion을 취하면서 측정한 measurement는 아래와 같다고 할 것이다. 아무래도 직선 운동이 아니다 보니까 상당히 더러워 보이는 숫자들이 나왔다. 그리고 무엇보다 중요한 것으로 이번 실습에서는 노이즈를 고려해 줄 것이다. 

 현재 자신의 각도 Θ에 대한 bearing noise, 앞바퀴가 돌아간 정도에 대한 steering noise, 앞으로 간 거리에 대한 distance noise들을 설정해 준다. 그리고 max steering angle이 있는데, 사실 앞에서 설명했어야 하지만, 이는 앞바퀴가 최고로 돌 수 있는 각도이다. 실제 자동차라고 생각하기 위해서 한 건가... 본론으로 돌아가서 앞서 만들었던 두 가지 코드들을 기존 Particle filter코드와 병합하는 것이 과제이다. 

그래서 위와 같은 main문이 작동하도록 해 주어야 한다. 사실 앞서 다 만들었어서 바로 원본을 보자.

# --------------
# USER INSTRUCTIONS
#
# Now you will put everything together.
#
# First make sure that your sense and move functions
# work as expected for the test cases provided at the
# bottom of the previous two programming assignments.
# Once you are satisfied, copy your sense and move
# definitions into the robot class on this page, BUT
# now include noise.
#
# A good way to include noise in the sense step is to
# add Gaussian noise, centered at zero with variance
# of self.bearing_noise to each bearing. You can do this
# with the command random.gauss(0, self.bearing_noise)
#
# In the move step, you should make sure that your
# actual steering angle is chosen from a Gaussian
# distribution of steering angles. This distribution
# should be centered at the intended steering angle
# with variance of self.steering_noise.
#
# Feel free to use the included set_noise function.
#
# Please do not modify anything except where indicated
# below.
 
from math import *
import random
 
# --------
# 
# some top level parameters
#
 
max_steering_angle = pi / 4.0 # You do not need to use this value, but keep in mind the limitations of a real car.
bearing_noise = 0.1 # Noise parameter: should be included in sense function.
steering_noise = 0.1 # Noise parameter: should be included in move function.
distance_noise = 5.0 # Noise parameter: should be included in move function.
 
tolerance_xy = 15.0 # Tolerance for localization in the x and y directions.
tolerance_orientation = 0.25 # Tolerance for orientation.
 
 
# --------
# 
# the "world" has 4 landmarks.
# the robot's initial coordinates are somewhere in the square
# represented by the landmarks.
#
# NOTE: Landmark coordinates are given in (y, x) form and NOT
# in the traditional (x, y) format!
 
landmarks  = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of 4 landmarks in (y, x) format.
world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel "out of bounds"
 
# ------------------------------------------------
# 
# this is the robot class
#
 
class robot:
 
    # --------
    # init: 
    #    creates robot and initializes location/orientation 
    #
 
    def __init__(self, length = 20.0):
        self.x = random.random() * world_size # initial x position
        self.y = random.random() * world_size # initial y position
        self.orientation = random.random() * 2.0 * pi # initial orientation
        self.length = length # length of robot
        self.bearing_noise  = 0.0 # initialize bearing noise to zero
        self.steering_noise = 0.0 # initialize steering noise to zero
        self.distance_noise = 0.0 # initialize distance noise to zero
 
    # --------
    # set: 
    #    sets a robot coordinate
    #
 
    def set(self, new_x, new_y, new_orientation):
 
        if new_orientation < 0 or new_orientation >= 2 * pi:
            raise ValueError, 'Orientation must be in [0..2pi]'
        self.x = float(new_x)
        self.y = float(new_y)
        self.orientation = float(new_orientation)
 
    # --------
    # set_noise: 
    #    sets the noise parameters
    #
    def set_noise(self, new_b_noise, new_s_noise, new_d_noise):
        # makes it possible to change the noise parameters
        # this is often useful in particle filters
        self.bearing_noise  = float(new_b_noise)
        self.steering_noise = float(new_s_noise)
        self.distance_noise = float(new_d_noise)
 
    # --------
    # measurement_prob
    #    computes the probability of a measurement
    #  
 
    def measurement_prob(self, measurements):
 
        # calculate the correct measurement
        predicted_measurements = self.sense(0) # Our sense function took 0 as an argument to switch off noise.
 
 
        # compute errors
        error = 1.0
        for i in range(len(measurements)):
            error_bearing = abs(measurements[i] - predicted_measurements[i])
            error_bearing = (error_bearing + pi) % (2.0 * pi) - pi # truncate
            
 
            # update Gaussian
            error *= (exp(- (error_bearing ** 2) / (self.bearing_noise ** 2) / 2.0) /  
                      sqrt(2.0 * pi * (self.bearing_noise ** 2)))
 
        return error
    
    def __repr__(self): #allows us to print robot attributes.
        return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), 
                                                str(self.orientation))
    
    ############# ONLY ADD/MODIFY CODE BELOW HERE ###################
       
    # --------
    # move: 
    #   
    def move(self, motion): # Do not change the name of this function
 
        # ADD CODE HERE
        steering = motion[0]
        distance = motion[1]
        
        if abs(steering) > max_steering_angle:
            raise ValueError
 
        if distance < 0.0:
            raise ValueError
 
        result = robot(20)
        result.length = self.length
        result.bearing_noise = self.bearing_noise
        result.steering_noise = self.steering_noise
        result.distance_noise = self.distance_noise
 
        steering2 = random.gauss(steering, self.steering_noise)
        distance2 = random.gauss(distance, self.distance_noise)
 
        turn = tan(steering2) * distance2 / self.length
 
        if abs(turn) < 0.001:
            result.x = self.x + distance2 * cos(self.orientation)
            result.y = self.y + distance2 * sin(self.orientation)
            result.orientation = (self.orientation + turn) % (2.0 * pi)
        else :
            R = distance / turn
            cx = self.x - R * sin(self.orientation)
            cy = self.y + R * cos(self.orientation)
            result.x = cx + R * sin(self.orientation + turn)
            result.y = cy - R * cos(self.orientation + turn)
            result.orientation = (self.orientation + turn) % (2. * pi)
        return result
    
    # copy your code from the previous exercise
    # and modify it so that it simulates motion noise
    # according to the noise parameters
    #           self.steering_noise
    #           self.distance_noise
 
    # --------
    # sense: 
    #    
 
    def sense(self, add_noise = 1): #do not change the name of this function
        Z = []
        for i in range(len(landmarks)):
            bearing = atan2(landmarks[i][0] - self.y, landmarks[i][1] - self.x) - self.orientation
            if add_noise:
               bearing += random.gauss(0.0, self.bearing_noise)
            Z.append(bearing % (2. * pi))
        # ENTER CODE HERE
        # HINT: You will probably need to use the function atan2()
 
        return Z #Leave this line here. Return vector Z of 4 bearings.
    # copy your code from the previous exercise
    # and modify it so that it simulates bearing noise
    # according to
    #           self.bearing_noise
 
    ############## ONLY ADD/MODIFY CODE ABOVE HERE ####################
 
# --------
#
# extract position from a particle set
# 
 
def get_position(p):
    x = 0.0
    y = 0.0
    orientation = 0.0
    for i in range(len(p)):
        x += p[i].x
        y += p[i].y
        # orientation is tricky because it is cyclic. By normalizing
        # around the first particle we are somewhat more robust to
        # the 0=2pi problem
        orientation += (((p[i].orientation - p[0].orientation + pi) % (2.0 * pi)) 
                        + p[0].orientation - pi)
    return [x / len(p), y / len(p), orientation / len(p)]
 
# --------
#
# The following code generates the measurements vector
# You can use it to develop your solution.
# 
 
 
def generate_ground_truth(motions):
 
    myrobot = robot()
    myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
 
    Z = []
    T = len(motions)
 
    for t in range(T):
        myrobot = myrobot.move(motions[t])
        Z.append(myrobot.sense())
    #print 'Robot:    ', myrobot
    return [myrobot, Z]
 
# --------
#
# The following code prints the measurements associated
# with generate_ground_truth
#
 
def print_measurements(Z):
 
    T = len(Z)
 
    print 'measurements = [[%.8s, %.8s, %.8s, %.8s],' % \
        (str(Z[0][0]), str(Z[0][1]), str(Z[0][2]), str(Z[0][3]))
    for t in range(1,T-1):
        print '                [%.8s, %.8s, %.8s, %.8s],' % \
            (str(Z[t][0]), str(Z[t][1]), str(Z[t][2]), str(Z[t][3]))
    print '                [%.8s, %.8s, %.8s, %.8s]]' % \
        (str(Z[T-1][0]), str(Z[T-1][1]), str(Z[T-1][2]), str(Z[T-1][3]))
 
# --------
#
# The following code checks to see if your particle filter
# localizes the robot to within the desired tolerances
# of the true position. The tolerances are defined at the top.
#
 
def check_output(final_robot, estimated_position):
 
    error_x = abs(final_robot.x - estimated_position[0])
    error_y = abs(final_robot.y - estimated_position[1])
    error_orientation = abs(final_robot.orientation - estimated_position[2])
    error_orientation = (error_orientation + pi) % (2.0 * pi) - pi
    correct = error_x < tolerance_xy and error_y < tolerance_xy \
              and error_orientation < tolerance_orientation
    return correct
 
 
 
def particle_filter(motions, measurements, N=500): # I know it's tempting, but don't change N!
    # --------
    #
    # Make particles
    # 
 
    p = []
    for i in range(N):
        r = robot()
        r.set_noise(bearing_noise, steering_noise, distance_noise)
        p.append(r)
 
    # --------
    #
    # Update particles
    #     
 
    for t in range(len(motions)):
    
        # motion update (prediction)
        p2 = []
        for i in range(N):
            p2.append(p[i].move(motions[t]))
        p = p2
 
        # measurement update
        w = []
        for i in range(N):
            w.append(p[i].measurement_prob(measurements[t]))
 
        # resampling
        p3 = []
        index = int(random.random() * N)
        beta = 0.0
        mw = max(w)
        for i in range(N):
            beta += random.random() * 2.0 * mw
            while beta > w[index]:
                beta -= w[index]
                index = (index + 1) % N
            p3.append(p[index])
        p = p3
    
    return get_position(p)
 
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out.
##
## You can test whether your particle filter works using the
## function check_output (see test case 2). We will be using a similar
## function. Note: Even for a well-implemented particle filter this
## function occasionally returns False. This is because a particle
## filter is a randomized algorithm. We will be testing your code
## multiple times. Make sure check_output returns True at least 80%
## of the time.
 
 
 
## --------
## TEST CASES:
## 
##1) Calling the particle_filter function with the following
##    motions and measurements should return a [x,y,orientation]
##    vector near [x=93.476 y=75.186 orient=5.2664], that is, the
##    robot's true location.
##
##motions = [[2. * pi / 10, 20.] for row in range(8)]
##measurements = [[4.746936, 3.859782, 3.045217, 2.045506],
##                [3.510067, 2.916300, 2.146394, 1.598332],
##                [2.972469, 2.407489, 1.588474, 1.611094],
##                [1.906178, 1.193329, 0.619356, 0.807930],
##                [1.352825, 0.662233, 0.144927, 0.799090],
##                [0.856150, 0.214590, 5.651497, 1.062401],
##                [0.194460, 5.660382, 4.761072, 2.471682],
##                [5.717342, 4.736780, 3.909599, 2.342536]]
##
##print particle_filter(motions, measurements)
 
## 2) You can generate your own test cases by generating
##    measurements using the generate_ground_truth function.
##    It will print the robot's last location when calling it.
##
##
##number_of_iterations = 6
##motions = [[2. * pi / 20, 12.] for row in range(number_of_iterations)]
##
##x = generate_ground_truth(motions)
##final_robot = x[0]
##measurements = x[1]
##estimated_position = particle_filter(motions, measurements)
##print_measurements(measurements)
##print 'Ground truth:    ', final_robot
##print 'Particle filter: ', estimated_position
##print 'Code check:      ', check_output(final_robot, estimated_position)
 
 
 
 

자, 너무나 친절한 주석질에 의해서 300줄이 넘어 버렸다... 우리가 주의 깊게 봐야 할 부분은 Particle filter함수이다. 

def particle_filter(motions, measurements, N=500): # I know it's tempting, but don't change N!
    # --------
    #
    # Make particles
    # 
    p = []
    for i in range(N):
        r = robot()
        r.set_noise(bearing_noise, steering_noise, distance_noise)
        p.append(r)
    # --------
    #
    # Update particles
    #     
    for t in range(len(motions)):
    
        # motion update (prediction)
        p2 = []
        for i in range(N):
            p2.append(p[i].move(motions[t]))
        p = p2
        # measurement update
        w = []
        for i in range(N):
            w.append(p[i].measurement_prob(measurements[t]))
        # resampling
        p3 = []
        index = int(random.random() * N)
        beta = 0.0
        mw = max(w)
        for i in range(N):
            beta += random.random() * 2.0 * mw
            while beta > w[index]:
                beta -= w[index]
                index = (index + 1) % N
            p3.append(p[index])
        p = p3
    
    return get_position(p)

 노이즈를 설정하고, motion의 시행 횟수만큼 반복문을 돌린다. 사실 motion과 measurement는 우리가 사용하는 모델이 바뀜에 따른 것이고 주의깊게 봐야 할 것은 Particle Filter의 전체적인 흐름이 어떻게 흘러가는지를 이해하는 것이다. 

    p = []
    for i in range(N):
        r = robot()
        r.set_noise(bearing_noise, steering_noise, distance_noise)
        p.append(r)

 N개의 Particle들을 생성하고 noise도 설정한다. 

    for t in range(len(motions)):

 Motion의 시행 횟수만큼 반복할 건데, 

        p2 = []
        for i in range(N):
            p2.append(p[i].move(motions[t]))
        p = p2

 로봇이 움직이는 만큼 각 Particle들도 움직여 준다. 

        # measurement update
        w = []
        for i in range(N):
            w.append(p[i].measurement_prob(measurements[t]))

 landmark들에 대해서 거리와 각도를 구하고 그에 따라 weight를 부여한다. weight는 실제 로봇의 위치에 가까울 수록 큰 값을 가진다. 

        # resampling
        p3 = []
        index = int(random.random() * N)
        beta = 0.0
        mw = max(w)
        for i in range(N):
            beta += random.random() * 2.0 * mw
            while beta > w[index]:
                beta -= w[index]
                index = (index + 1) % N
            p3.append(p[index])
        p = p3

 이 weight값들을 가지고 N개의 Particle들을 Resampling한다.

다음은 Particle Filter에서의 질문들에 대한 Q & A 이다.

Q : 아래와 같이 룰렛 알고리즘을 할 때, 가장 많은 자리를 차지하는 부분의 2배 만큼 반복해서 움직이게 했는데 이렇게 한 이유가 있는가?? 

A : 모름, 그냥 적당한 수로 했다고 한다. 너무 크면 시간도 많이 걸리고 해서 적당히 했다고 한다. 

Q : 언제 Particle Filter를 쓰고 언제 Kalman Filter를 쓰는가? 

A : Particle Filter는 구현은 싑지만 차원이 늘어남에 따라 복잡도가 증가한다. 그래서 15차원 막 이러면 보통 Kalman Filter를 쓰는데, Kalman은 아시다시피 1개의 가장 높은 확률만 알 수 있는 것이 문제이다. 그래서 Extended된 Kalman Filter(mixed rough Kalman filter, multihypothesis Kalman filter)들을 쓰곤 한다.

 multimodal distribution을 가지고 있다, 그럼 particle filters를 쓰고, continuous space이고 unimodal distribution을 가지고 있다, 그럼 Kalman filter를 써라 그리고 이 둘을 병합한 rao-blackwellized filter라는 것도 있다.

Q : Particle들의 개수 N을 어떤 기준으로??

A : 필터를 잘 설계했다면 적은 갯수로도 충분하다. 더욱이, 로봇 시스템은 Real-time이고, 특정 cycle안에 추적하지 않으면 의미가 없다. 보통 잘 추적되고 있는지 보고 싶을 때 Normalized되지 않은 weight값들을 보는데, 로봇의 실제 위치와 비슷한 Particle이 충분히 큰 weight를 가지고 있다면 잘 하고 있는 것이고, 그렇지 않다면 뽑는 Particle들의 수를 조금 더 늘려야 할 것이다. 

Q : landmark들을 사용해서 실제 위치와 얼마나 유사한지 보았었는데 실제로 자율 주행 차량들에 이용한다고 할 때, 이 landmark들이 움직이는 사람인지, 가만히 있는 표지판인지 어떻게 알 수 있는가??

A : 우선, 자동차라고 가정했을 때, 자신이 얼마나 움직였는지는 알 수가 있다. 그리고 time step들을 계속 거치면서, time step 이전과 이후에 대해 어떠한 변화가 있었는지 알아볼 수가 있다. 이를 통해 움직인 물체인지, 아닌지 알 수가 있다. 

 예를 들면, 도로 위의 표지판이라고 하면 내가 움직인 그 정도만 보이는 위치가 달라진다. 그런데 만약, 나에게 달려오는 고라니라고 하면, 갑자기 크기가 커지는 등으로 변화가 있을 것이다. 사실 움직임과 measurement의 노이즈에 의해서 구별하기 쉽지 않을까 싶기도 한데, 그 노이즈를 고려하더라고 확실하게 변화가 큰 것들은 알아챌 수가 있을 것이다. 

 길고 긴 포스트가 끝났다... 사진도 많고 양도 너무 많아서 글을 쓰는데 버퍼링이 걸릴 정도... 다음에는 보다 순수 알고리즘적인 Search에 대해서 다루어 본다!!