SLAM 공부했다.

오우~ 대망의 SLAM이다. 그리고 이 강좌 (cs373)의 마지막 챕터이기도 하다.

SLAM(Simultaneous Localization And Mapping)은 지금까지 배웠던 모든 개념들이 한 번에 들어가 있는 것이라 해도 무관하다. 로봇을 어떤 장소에 던져 놓았다고 할 때, 이 로봇은 자기가 지금 어디 있는지 확인할 수 있는 방법이 없다.(GPS가 있다면 모르지만 이조차도 오차가 있기에 정확하게 알기는 힘들다.)

 계속 움직이며 기준이 되는 어떤 장소의 관찰을 한다면, 자신이 어디에 있는지 대충 알 수 있을 것이다. 이 과정이 Filter에 해당한다. 그럼, 로봇을 어디에 던져주면서, 어떤 임무를 수행하게 하기 위해서는 경로를 지정해 주어야 할 것이고 각 어떠한 움직임을 취하라는 명령도 해 주어야 한다. 이것들이 Search와 Control이었다. 

 그런데 사실, 그동안 우리가 보았던 것들은 모두 지도가 주어져 있고, 절대적인 좌표을 알 수 있다는 상황을 가정했었다. 그렇다면 만약 지도가 없을 때에는 어떻게 알 수 있을까? 이에 대한 문제를 해결하는 것이 SLAM의 과제이다. 

 본론으로 들어가기에 앞서서, 우리의 자동차 모델을 보다 유연하게 만들어 보자. 

 

 

 기존에 사용하던 모델은 noise의 영향으로 인해서 오른쪽과 같이 코너링을 할 때, 충돌이 발생할 우려가 있다. 우선적으로 이 문제를 고치고 시작하려 한다. 

 

 

 문제는 이거다. 왼쪽 그림과 같이 뚝뚝 끊긴 경로를 설정했는데, 실제로 로봇이 이렇게 갈 수 있을리 만무하다. 그래서 어느 지점부터는 다음 부분을 따르도록 switching을 시켜햐 하는데, 이 과정을 해결해야 함과 동시에, 직선 경로라서 다행이긴 하지만, Error를 계산하는 방법을 달리 해야 할 것이다. 경로와의 y거리가 아닌, 경로와의 최단 거리를 최소화하는쪽으로 Controll을 해야 한다. 

 직선과 점 사이의 거리 공식을 이용할 것인데, 간단히 말해서 벡터의 내적(inner product)가 사용되었다고 할 수 있다 그래서 식의 모양이 오른쪽 그림과 같은 것이다. CTE=~~~ 라고 쓰여 있는 부분이 실제 Error로 쓰일 부분이다. 여기서는 자세한 수학적 설명은 배제하고 싶어 일단은 상황에 따라 CTE를 잘 설계해야 함을 강조하고만 넘어가겠다. 

 아 , 그리고 문제 하단에는 이렇게 쓰여 있다.

Note: the formula for CTE should actually take the square root of the denominator, i.e. (Ry dx - Rx dy) / sqrt(dx^2 + dy^2), but if you implement it as shown in lecture it will work well enough to pass the grader.

 분모에 나누어주는 항에 루트를 씌워야 하는 것이 원칙이긴 하나, 연산의 최소화를 위해서 하지 않은 듯 하다. 연습 문제의 답이 포함된 코드를 남겨 놓겠다. 500줄 정도 되다 보니 그냥 답까지 한 번에 올린다. 

# -----------
# User Instructions
#
# Familiarize yourself with the code below. Most of it
# reproduces results that you have obtained at some
# point in this class. Once you understand the code,
# write a function, cte, in the run class that
# computes the crosstrack
# error for the case of a segmented path. You will
# need to include the equations shown in the video.
#
 
from math import *
import random
 
 
# don't change the noise paameters
 
steering_noise    = 0.1
distance_noise    = 0.03
measurement_noise = 0.3
 
 
class plan:
 
    # --------
    # init: 
    #    creates an empty plan
    #
 
    def __init__(self, grid, init, goal, cost = 1):
        self.cost = cost
        self.grid = grid
        self.init = init
        self.goal = goal
        self.make_heuristic(grid, goal, self.cost)
        self.path = []
        self.spath = []
 
    # --------
    #
    # make heuristic function for a grid
        
    def make_heuristic(self, grid, goal, cost):
        self.heuristic = [[0 for row in range(len(grid[0]))] 
                          for col in range(len(grid))]
        for i in range(len(self.grid)):    
            for j in range(len(self.grid[0])):
                self.heuristic[i][j] = abs(i - self.goal[0]) + \
                    abs(j - self.goal[1])
 
 
 
    # ------------------------------------------------
    # 
    # A* for searching a path to the goal
    #
    #
 
    def astar(self):
 
 
        if self.heuristic == []:
            raise ValueError, "Heuristic must be defined to run A*"
 
        # internal motion parameters
        delta = [[-1,  0], # go up
                 [ 0,  -1], # go left
                 [ 1,  0], # go down
                 [ 0,  1]] # do right
 
 
        # open list elements are of the type: [f, g, h, x, y]
 
        closed = [[0 for row in range(len(self.grid[0]))] 
                  for col in range(len(self.grid))]
        action = [[0 for row in range(len(self.grid[0]))] 
                  for col in range(len(self.grid))]
 
        closed[self.init[0]][self.init[1]] = 1
 
 
        x = self.init[0]
        y = self.init[1]
        h = self.heuristic[x][y]
        g = 0
        f = g + h
 
        open = [[f, g, h, x, y]]
 
        found  = False # flag that is set when search complete
        resign = False # flag set if we can't find expand
        count  = 0
 
 
        while not found and not resign:
 
            # check if we still have elements on the open list
            if len(open) == 0:
                resign = True
                print '###### Search terminated without success'
                
            else:
                # remove node from list
                open.sort()
                open.reverse()
                next = open.pop()
                x = next[3]
                y = next[4]
                g = next[1]
 
            # check if we are done
 
            if x == goal[0] and y == goal[1]:
                found = True
                # print '###### A* search successful'
 
            else:
                # expand winning element and add to new open list
                for i in range(len(delta)):
                    x2 = x + delta[i][0]
                    y2 = y + delta[i][1]
                    if x2 >= 0 and x2 < len(self.grid) and y2 >= 0 \
                            and y2 < len(self.grid[0]):
                        if closed[x2][y2] == 0 and self.grid[x2][y2] == 0:
                            g2 = g + self.cost
                            h2 = self.heuristic[x2][y2]
                            f2 = g2 + h2
                            open.append([f2, g2, h2, x2, y2])
                            closed[x2][y2] = 1
                            action[x2][y2] = i
 
            count += 1
 
        # extract the path
 
 
 
        invpath = []
        x = self.goal[0]
        y = self.goal[1]
        invpath.append([x, y])
        while x != self.init[0] or y != self.init[1]:
            x2 = x - delta[action[x][y]][0]
            y2 = y - delta[action[x][y]][1]
            x = x2
            y = y2
            invpath.append([x, y])
 
        self.path = []
        for i in range(len(invpath)):
            self.path.append(invpath[len(invpath) - 1 - i])
 
 
 
 
    # ------------------------------------------------
    # 
    # this is the smoothing function
    #
 
  
 
 
    def smooth(self, weight_data = 0.1, weight_smooth = 0.1, 
               tolerance = 0.000001):
 
        if self.path == []:
            raise ValueError, "Run A* first before smoothing path"
 
        self.spath = [[0 for row in range(len(self.path[0]))] \
                           for col in range(len(self.path))]
        for i in range(len(self.path)):
            for j in range(len(self.path[0])):
                self.spath[i][j] = self.path[i][j]
 
        change = tolerance
        while change >= tolerance:
            change = 0.0
            for i in range(1, len(self.path)-1):
                for j in range(len(self.path[0])):
                    aux = self.spath[i][j]
                    
                    self.spath[i][j] += weight_data * \
                        (self.path[i][j] - self.spath[i][j])
                    
                    self.spath[i][j] += weight_smooth * \
                        (self.spath[i-1][j] + self.spath[i+1][j] 
                         - (2.0 * self.spath[i][j]))
                    if i >= 2:
                        self.spath[i][j] += 0.5 * weight_smooth * \
                            (2.0 * self.spath[i-1][j] - self.spath[i-2][j] 
                             - self.spath[i][j])
                    if i <= len(self.path) - 3:
                        self.spath[i][j] += 0.5 * weight_smooth * \
                            (2.0 * self.spath[i+1][j] - self.spath[i+2][j] 
                             - self.spath[i][j])
                
            change += abs(aux - self.spath[i][j])
                
 
 
 
 
 
 
# ------------------------------------------------
# 
# this is the robot class
#
 
class robot:
 
    # --------
    # init: 
    #    creates robot and initializes location/orientation to 0, 0, 0
    #
 
    def __init__(self, length = 0.5):
        self.x = 0.0
        self.y = 0.0
        self.orientation = 0.0
        self.length = length
        self.steering_noise    = 0.0
        self.distance_noise    = 0.0
        self.measurement_noise = 0.0
        self.num_collisions    = 0
        self.num_steps         = 0
 
    # --------
    # set: 
    #    sets a robot coordinate
    #
 
    def set(self, new_x, new_y, new_orientation):
 
        self.x = float(new_x)
        self.y = float(new_y)
        self.orientation = float(new_orientation) % (2.0 * pi)
 
 
    # --------
    # set_noise: 
    #    sets the noise parameters
    #
 
    def set_noise(self, new_s_noise, new_d_noise, new_m_noise):
        # makes it possible to change the noise parameters
        # this is often useful in particle filters
        self.steering_noise     = float(new_s_noise)
        self.distance_noise    = float(new_d_noise)
        self.measurement_noise = float(new_m_noise)
 
    # --------
    # check: 
    #    checks of the robot pose collides with an obstacle, or
    # is too far outside the plane
 
    def check_collision(self, grid):
        for i in range(len(grid)):
            for j in range(len(grid[0])):
                if grid[i][j] == 1:
                    dist = sqrt((self.x - float(i)) ** 2 + 
                                (self.y - float(j)) ** 2)
                    if dist < 0.5:
                        self.num_collisions += 1
                        return False
        return True
        
    def check_goal(self, goal, threshold = 1.0):
        dist =  sqrt((float(goal[0]) - self.x) ** 2 + (float(goal[1]) - self.y) ** 2)
        return dist < threshold
        
    # --------
    # move: 
    #    steering = front wheel steering angle, limited by max_steering_angle
    #    distance = total distance driven, most be non-negative
 
    def move(self, grid, steering, distance, 
             tolerance = 0.001, max_steering_angle = pi / 4.0):
 
        if steering > max_steering_angle:
            steering = max_steering_angle
        if steering < -max_steering_angle:
            steering = -max_steering_angle
        if distance < 0.0:
            distance = 0.0
 
 
        # make a new copy
        res = robot()
        res.length            = self.length
        res.steering_noise    = self.steering_noise
        res.distance_noise    = self.distance_noise
        res.measurement_noise = self.measurement_noise
        res.num_collisions    = self.num_collisions
        res.num_steps         = self.num_steps + 1
 
        # apply noise
        steering2 = random.gauss(steering, self.steering_noise)
        distance2 = random.gauss(distance, self.distance_noise)
 
 
        # Execute motion
        turn = tan(steering2) * distance2 / res.length
 
        if abs(turn) < tolerance:
 
            # approximate by straight line motion
 
            res.x = self.x + (distance2 * cos(self.orientation))
            res.y = self.y + (distance2 * sin(self.orientation))
            res.orientation = (self.orientation + turn) % (2.0 * pi)
 
        else:
 
            # approximate bicycle model for motion
 
            radius = distance2 / turn
            cx = self.x - (sin(self.orientation) * radius)
            cy = self.y + (cos(self.orientation) * radius)
            res.orientation = (self.orientation + turn) % (2.0 * pi)
            res.x = cx + (sin(res.orientation) * radius)
            res.y = cy - (cos(res.orientation) * radius)
 
        # check for collision
        # res.check_collision(grid)
 
        return res
 
    # --------
    # sense: 
    #    
 
    def sense(self):
 
        return [random.gauss(self.x, self.measurement_noise),
                random.gauss(self.y, self.measurement_noise)]
 
    # --------
    # measurement_prob
    #    computes the probability of a measurement
    # 
 
    def measurement_prob(self, measurement):
 
        # compute errors
        error_x = measurement[0] - self.x
        error_y = measurement[1] - self.y
 
        # calculate Gaussian
        error = exp(- (error_x ** 2) / (self.measurement_noise ** 2) / 2.0) \
            / sqrt(2.0 * pi * (self.measurement_noise ** 2))
        error *= exp(- (error_y ** 2) / (self.measurement_noise ** 2) / 2.0) \
            / sqrt(2.0 * pi * (self.measurement_noise ** 2))
 
        return error
 
 
 
    def __repr__(self):
        # return '[x=%.5f y=%.5f orient=%.5f]'  % (self.x, self.y, self.orientation)
        return '[%.5f, %.5f]'  % (self.x, self.y)
 
 
 
 
 
 
# ------------------------------------------------
# 
# this is the particle filter class
#
 
class particles:
 
    # --------
    # init: 
    #    creates particle set with given initial position
    #
 
    def __init__(self, x, y, theta, 
                 steering_noise, distance_noise, measurement_noise, N = 100):
        self.N = N
        self.steering_noise    = steering_noise
        self.distance_noise    = distance_noise
        self.measurement_noise = measurement_noise
        
        self.data = []
        for i in range(self.N):
            r = robot()
            r.set(x, y, theta)
            r.set_noise(steering_noise, distance_noise, measurement_noise)
            self.data.append(r)
 
 
    # --------
    #
    # extract position from a particle set
    # 
    
    def get_position(self):
        x = 0.0
        y = 0.0
        orientation = 0.0
 
        for i in range(self.N):
            x += self.data[i].x
            y += self.data[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 += (((self.data[i].orientation
                              - self.data[0].orientation + pi) % (2.0 * pi)) 
                            + self.data[0].orientation - pi)
        return [x / self.N, y / self.N, orientation / self.N]
 
    # --------
    #
    # motion of the particles
    # 
 
    def move(self, grid, steer, speed):
        newdata = []
 
        for i in range(self.N):
            r = self.data[i].move(grid, steer, speed)
            newdata.append(r)
        self.data = newdata
 
    # --------
    #
    # sensing and resampling
    # 
 
    def sense(self, Z):
        w = []
        for i in range(self.N):
            w.append(self.data[i].measurement_prob(Z))
 
        # resampling (careful, this is using shallow copy)
        p3 = []
        index = int(random.random() * self.N)
        beta = 0.0
        mw = max(w)
 
        for i in range(self.N):
            beta += random.random() * 2.0 * mw
            while beta > w[index]:
                beta -= w[index]
                index = (index + 1) % self.N
            p3.append(self.data[index])
        self.data = p3
 
    
 
 
 
    
 
# --------
#
# run:  runs control program for the robot
#
 
 
def run(grid, goal, spath, params, printflag = False, speed = 0.1, timeout = 1000):
 
    myrobot = robot()
    myrobot.set(0., 0., 0.)
    myrobot.set_noise(steering_noise, distance_noise, measurement_noise)
    filter = particles(myrobot.x, myrobot.y, myrobot.orientation,
                       steering_noise, distance_noise, measurement_noise)
 
    cte  = 0.0
    err  = 0.0
    N    = 0
 
    index = 0 # index into the path
    
    while not myrobot.check_goal(goal) and N < timeout:
 
        diff_cte = - cte
 
 
        # ----------------------------------------
        # compute the CTE
 
        # start with the present robot estimate
        estimate = filter.get_position()
 
        ### ENTER CODE HERE
        delta_x = spath[index + 1][0] - spath[index][0]
        delta_y = spath[index + 1][1] - spath[index][1]
        R_x = estimate[0] - spath[index][0]
        R_y = estimate[1] - spath[index][1]
        u = (R_x * delta_x + R_y * delta_y) / (delta_x * delta_x + delta_y * delta_y)
        cte = (R_y * delta_x - R_x * delta_y) / (delta_x * delta_x + delta_y * delta_y)
        if u > 1.0:
            index += 1
 
        # ----------------------------------------
 
 
        diff_cte += cte
 
        steer = - params[0] * cte - params[1] * diff_cte 
 
        myrobot = myrobot.move(grid, steer, speed)
        filter.move(grid, steer, speed)
 
        Z = myrobot.sense()
        filter.sense(Z)
 
        if not myrobot.check_collision(grid):
            print '##### Collision ####'
 
        err += (cte ** 2)
        N += 1
 
        if printflag:
            print myrobot, cte, index, u
 
    return [myrobot.check_goal(goal), myrobot.num_collisions, myrobot.num_steps]
 
 
# ------------------------------------------------
# 
# this is our main routine
#
 
def main(grid, init, goal, steering_noise, distance_noise, measurement_noise, 
     weight_data, weight_smooth, p_gain, d_gain):
 
    path = plan(grid, init, goal)
    path.astar()
    path.smooth(weight_data, weight_smooth)
    return run(grid, goal, path.spath, [p_gain, d_gain])
 
    
 
 
# ------------------------------------------------
# 
# input data and parameters
#
 
 
# grid format:
#   0 = navigable space
#   1 = occupied space
 
grid = [[0, 1, 0, 0, 0, 0],
        [0, 1, 0, 1, 1, 0],
        [0, 1, 0, 1, 0, 0],
        [0, 0, 0, 1, 0, 1],
        [0, 1, 0, 1, 0, 0]]
 
 
init = [0, 0]
goal = [len(grid)-1, len(grid[0])-1]
 
 
steering_noise    = 0.1
distance_noise    = 0.03
measurement_noise = 0.3
 
weight_data       = 0.1
weight_smooth     = 0.2
p_gain            = 2.0
d_gain            = 6.0
 
    
print main(grid, init, goal, steering_noise, distance_noise, measurement_noise, 
           weight_data, weight_smooth, p_gain, d_gain)
 
 
 
 
def twiddle(init_params):
    n_params   = len(init_params)
    dparams    = [1.0 for row in range(n_params)]
    params     = [0.0 for row in range(n_params)]
    K = 10
 
    for i in range(n_params):
        params[i] = init_params[i]
 
 
    best_error = 0.0;
    for k in range(K):
        ret = main(grid, init, goal, 
                   steering_noise, distance_noise, measurement_noise, 
                   params[0], params[1], params[2], params[3])
        if ret[0]:
            best_error += ret[1] * 100 + ret[2]
        else:
            best_error += 99999
    best_error = float(best_error) / float(k+1)
    print best_error
 
    n = 0
    while sum(dparams) > 0.0000001:
        for i in range(len(params)):
            params[i] += dparams[i]
            err = 0
            for k in range(K):
                ret = main(grid, init, goal, 
                           steering_noise, distance_noise, measurement_noise, 
                           params[0], params[1], params[2], params[3], best_error)
                if ret[0]:
                    err += ret[1] * 100 + ret[2]
                else:
                    err += 99999
            print float(err) / float(k+1)
            if err < best_error:
                best_error = float(err) / float(k+1)
                dparams[i] *= 1.1
            else:
                params[i] -= 2.0 * dparams[i]            
                err = 0
                for k in range(K):
                    ret = main(grid, init, goal, 
                               steering_noise, distance_noise, measurement_noise, 
                               params[0], params[1], params[2], params[3], best_error)
                    if ret[0]:
                        err += ret[1] * 100 + ret[2]
                    else:
                        err += 99999
                print float(err) / float(k+1)
                if err < best_error:
                    best_error = float(err) / float(k+1)
                    dparams[i] *= 1.1
                else:
                    params[i] += dparams[i]
                    dparams[i] *= 0.5
        n += 1
        print 'Twiddle #', n, params, ' -> ', best_error
    print ' '
    return params
 
 
#twiddle([weight_data, weight_smooth, p_gain, d_gain])
 

        estimate = filter.get_position()

 

        ### ENTER CODE HERE

        delta_x = spath[index + 1][0] - spath[index][0]

        delta_y = spath[index + 1][1] - spath[index][1]

        R_x = estimate[0] - spath[index][0]

        R_y = estimate[1] - spath[index][1]

        u = (R_x * delta_x + R_y * delta_y) / (delta_x * delta_x + delta_y * delta_y)

        cte = (R_y * delta_x - R_x * delta_y) / (delta_x * delta_x + delta_y * delta_y)

        if u > 1.0:

            index += 1

 핵심은 위 부분이다. 현재 위치로 추정되는 좌표를 받고, Error를 새로 계산한 다음, 이를 이용했다. 다음으로는 gain값들을 조절하면서, 최적의 경우를 찾아 보시라고 한다. 583-586의 값들을 조정하면서 step을 최소로 하되, collision이 생기지는 않도록 해 보시라.

# -----------
# User Instructions
#
# The point of this exercise is to find the optimal
# parameters! You can write a twiddle function or you
# can use any other method
# that you like. Since we don't know what the optimal
# parameters are, we will be very loose with the 
# grading. If you find parameters that work well, post 
# them in the forums!
#
# Note: when we first released this problem, we 
# included a twiddle function. But that's no fun!
# Try coding up your own parameter optimization
# and see how quickly you can get to the goal.
#
# You can find the parameters at line 581.
 
from math import *
import random
 
 
# don't change the noise paameters
 
steering_noise    = 0.1
distance_noise    = 0.03
measurement_noise = 0.3
 
 
class plan:
 
    # --------
    # init: 
    #    creates an empty plan
    #
 
    def __init__(self, grid, init, goal, cost = 1):
        self.cost = cost
        self.grid = grid
        self.init = init
        self.goal = goal
        self.make_heuristic(grid, goal, self.cost)
        self.path = []
        self.spath = []
 
    # --------
    #
    # make heuristic function for a grid
        
    def make_heuristic(self, grid, goal, cost):
        self.heuristic = [[0 for row in range(len(grid[0]))] 
                          for col in range(len(grid))]
        for i in range(len(self.grid)):    
            for j in range(len(self.grid[0])):
                self.heuristic[i][j] = abs(i - self.goal[0]) + \
                    abs(j - self.goal[1])
 
 
 
    # ------------------------------------------------
    # 
    # A* for searching a path to the goal
    #
    #
 
    def astar(self):
 
 
        if self.heuristic == []:
            raise ValueError, "Heuristic must be defined to run A*"
 
        # internal motion parameters
        delta = [[-1,  0], # go up
                 [ 0,  -1], # go left
                 [ 1,  0], # go down
                 [ 0,  1]] # do right
 
 
        # open list elements are of the type: [f, g, h, x, y]
 
        closed = [[0 for row in range(len(self.grid[0]))] 
                  for col in range(len(self.grid))]
        action = [[0 for row in range(len(self.grid[0]))] 
                  for col in range(len(self.grid))]
 
        closed[self.init[0]][self.init[1]] = 1
 
 
        x = self.init[0]
        y = self.init[1]
        h = self.heuristic[x][y]
        g = 0
        f = g + h
 
        open = [[f, g, h, x, y]]
 
        found  = False # flag that is set when search complete
        resign = False # flag set if we can't find expand
        count  = 0
 
 
        while not found and not resign:
 
            # check if we still have elements on the open list
            if len(open) == 0:
                resign = True
                print '###### Search terminated without success'
                
            else:
                # remove node from list
                open.sort()
                open.reverse()
                next = open.pop()
                x = next[3]
                y = next[4]
                g = next[1]
 
            # check if we are done
 
            if x == goal[0] and y == goal[1]:
                found = True
                # print '###### A* search successful'
 
            else:
                # expand winning element and add to new open list
                for i in range(len(delta)):
                    x2 = x + delta[i][0]
                    y2 = y + delta[i][1]
                    if x2 >= 0 and x2 < len(self.grid) and y2 >= 0 \
                            and y2 < len(self.grid[0]):
                        if closed[x2][y2] == 0 and self.grid[x2][y2] == 0:
                            g2 = g + self.cost
                            h2 = self.heuristic[x2][y2]
                            f2 = g2 + h2
                            open.append([f2, g2, h2, x2, y2])
                            closed[x2][y2] = 1
                            action[x2][y2] = i
 
            count += 1
 
        # extract the path
 
 
 
        invpath = []
        x = self.goal[0]
        y = self.goal[1]
        invpath.append([x, y])
        while x != self.init[0] or y != self.init[1]:
            x2 = x - delta[action[x][y]][0]
            y2 = y - delta[action[x][y]][1]
            x = x2
            y = y2
            invpath.append([x, y])
 
        self.path = []
        for i in range(len(invpath)):
            self.path.append(invpath[len(invpath) - 1 - i])
 
 
 
 
    # ------------------------------------------------
    # 
    # this is the smoothing function
    #
 
  
 
 
    def smooth(self, weight_data = 0.1, weight_smooth = 0.1, 
               tolerance = 0.000001):
 
        if self.path == []:
            raise ValueError, "Run A* first before smoothing path"
 
        self.spath = [[0 for row in range(len(self.path[0]))] \
                           for col in range(len(self.path))]
        for i in range(len(self.path)):
            for j in range(len(self.path[0])):
                self.spath[i][j] = self.path[i][j]
 
        change = tolerance
        while change >= tolerance:
            change = 0.0
            for i in range(1, len(self.path)-1):
                for j in range(len(self.path[0])):
                    aux = self.spath[i][j]
                    
                    self.spath[i][j] += weight_data * \
                        (self.path[i][j] - self.spath[i][j])
                    
                    self.spath[i][j] += weight_smooth * \
                        (self.spath[i-1][j] + self.spath[i+1][j] 
                         - (2.0 * self.spath[i][j]))
                    if i >= 2:
                        self.spath[i][j] += 0.5 * weight_smooth * \
                            (2.0 * self.spath[i-1][j] - self.spath[i-2][j] 
                             - self.spath[i][j])
                    if i <= len(self.path) - 3:
                        self.spath[i][j] += 0.5 * weight_smooth * \
                            (2.0 * self.spath[i+1][j] - self.spath[i+2][j] 
                             - self.spath[i][j])
                
            change += abs(aux - self.spath[i][j])
                
 
 
 
 
 
 
# ------------------------------------------------
# 
# this is the robot class
#
 
class robot:
 
    # --------
    # init: 
    #    creates robot and initializes location/orientation to 0, 0, 0
    #
 
    def __init__(self, length = 0.5):
        self.x = 0.0
        self.y = 0.0
        self.orientation = 0.0
        self.length = length
        self.steering_noise    = 0.0
        self.distance_noise    = 0.0
        self.measurement_noise = 0.0
        self.num_collisions    = 0
        self.num_steps         = 0
 
    # --------
    # set: 
    #    sets a robot coordinate
    #
 
    def set(self, new_x, new_y, new_orientation):
 
        self.x = float(new_x)
        self.y = float(new_y)
        self.orientation = float(new_orientation) % (2.0 * pi)
 
 
    # --------
    # set_noise: 
    #    sets the noise parameters
    #
 
    def set_noise(self, new_s_noise, new_d_noise, new_m_noise):
        # makes it possible to change the noise parameters
        # this is often useful in particle filters
        self.steering_noise     = float(new_s_noise)
        self.distance_noise    = float(new_d_noise)
        self.measurement_noise = float(new_m_noise)
 
    # --------
    # check: 
    #    checks of the robot pose collides with an obstacle, or
    # is too far outside the plane
 
    def check_collision(self, grid):
        for i in range(len(grid)):
            for j in range(len(grid[0])):
                if grid[i][j] == 1:
                    dist = sqrt((self.x - float(i)) ** 2 + 
                                (self.y - float(j)) ** 2)
                    if dist < 0.5:
                        self.num_collisions += 1
                        return False
        return True
        
    def check_goal(self, goal, threshold = 1.0):
        dist =  sqrt((float(goal[0]) - self.x) ** 2 + (float(goal[1]) - self.y) ** 2)
        return dist < threshold
        
    # --------
    # move: 
    #    steering = front wheel steering angle, limited by max_steering_angle
    #    distance = total distance driven, most be non-negative
 
    def move(self, grid, steering, distance, 
             tolerance = 0.001, max_steering_angle = pi / 4.0):
 
        if steering > max_steering_angle:
            steering = max_steering_angle
        if steering < -max_steering_angle:
            steering = -max_steering_angle
        if distance < 0.0:
            distance = 0.0
 
 
        # make a new copy
        res = robot()
        res.length            = self.length
        res.steering_noise    = self.steering_noise
        res.distance_noise    = self.distance_noise
        res.measurement_noise = self.measurement_noise
        res.num_collisions    = self.num_collisions
        res.num_steps         = self.num_steps + 1
 
        # apply noise
        steering2 = random.gauss(steering, self.steering_noise)
        distance2 = random.gauss(distance, self.distance_noise)
 
 
        # Execute motion
        turn = tan(steering2) * distance2 / res.length
 
        if abs(turn) < tolerance:
 
            # approximate by straight line motion
 
            res.x = self.x + (distance2 * cos(self.orientation))
            res.y = self.y + (distance2 * sin(self.orientation))
            res.orientation = (self.orientation + turn) % (2.0 * pi)
 
        else:
 
            # approximate bicycle model for motion
 
            radius = distance2 / turn
            cx = self.x - (sin(self.orientation) * radius)
            cy = self.y + (cos(self.orientation) * radius)
            res.orientation = (self.orientation + turn) % (2.0 * pi)
            res.x = cx + (sin(res.orientation) * radius)
            res.y = cy - (cos(res.orientation) * radius)
 
        # check for collision
        # res.check_collision(grid)
 
        return res
 
    # --------
    # sense: 
    #    
 
    def sense(self):
 
        return [random.gauss(self.x, self.measurement_noise),
                random.gauss(self.y, self.measurement_noise)]
 
    # --------
    # measurement_prob
    #    computes the probability of a measurement
    # 
 
    def measurement_prob(self, measurement):
 
        # compute errors
        error_x = measurement[0] - self.x
        error_y = measurement[1] - self.y
 
        # calculate Gaussian
        error = exp(- (error_x ** 2) / (self.measurement_noise ** 2) / 2.0) \
            / sqrt(2.0 * pi * (self.measurement_noise ** 2))
        error *= exp(- (error_y ** 2) / (self.measurement_noise ** 2) / 2.0) \
            / sqrt(2.0 * pi * (self.measurement_noise ** 2))
 
        return error
 
 
 
    def __repr__(self):
        # return '[x=%.5f y=%.5f orient=%.5f]'  % (self.x, self.y, self.orientation)
        return '[%.5f, %.5f]'  % (self.x, self.y)
 
 
 
 
 
 
# ------------------------------------------------
# 
# this is the particle filter class
#
 
class particles:
 
    # --------
    # init: 
    #    creates particle set with given initial position
    #
 
    def __init__(self, x, y, theta, 
                 steering_noise, distance_noise, measurement_noise, N = 100):
        self.N = N
        self.steering_noise    = steering_noise
        self.distance_noise    = distance_noise
        self.measurement_noise = measurement_noise
        
        self.data = []
        for i in range(self.N):
            r = robot()
            r.set(x, y, theta)
            r.set_noise(steering_noise, distance_noise, measurement_noise)
            self.data.append(r)
 
 
    # --------
    #
    # extract position from a particle set
    # 
    
    def get_position(self):
        x = 0.0
        y = 0.0
        orientation = 0.0
 
        for i in range(self.N):
            x += self.data[i].x
            y += self.data[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 += (((self.data[i].orientation
                              - self.data[0].orientation + pi) % (2.0 * pi)) 
                            + self.data[0].orientation - pi)
        return [x / self.N, y / self.N, orientation / self.N]
 
    # --------
    #
    # motion of the particles
    # 
 
    def move(self, grid, steer, speed):
        newdata = []
 
        for i in range(self.N):
            r = self.data[i].move(grid, steer, speed)
            newdata.append(r)
        self.data = newdata
 
    # --------
    #
    # sensing and resampling
    # 
 
    def sense(self, Z):
        w = []
        for i in range(self.N):
            w.append(self.data[i].measurement_prob(Z))
 
        # resampling (careful, this is using shallow copy)
        p3 = []
        index = int(random.random() * self.N)
        beta = 0.0
        mw = max(w)
 
        for i in range(self.N):
            beta += random.random() * 2.0 * mw
            while beta > w[index]:
                beta -= w[index]
                index = (index + 1) % self.N
            p3.append(self.data[index])
        self.data = p3
 
    
 
 
 
    
 
# --------
#
# run:  runs control program for the robot
#
 
 
def run(grid, goal, spath, params, printflag = False, speed = 0.1, timeout = 1000):
 
    myrobot = robot()
    myrobot.set(0., 0., 0.)
    myrobot.set_noise(steering_noise, distance_noise, measurement_noise)
    filter = particles(myrobot.x, myrobot.y, myrobot.orientation,
                       steering_noise, distance_noise, measurement_noise)
 
    cte  = 0.0
    err  = 0.0
    N    = 0
 
    index = 0 # index into the path
    
    while not myrobot.check_goal(goal) and N < timeout:
 
        diff_cte = - cte
 
 
        # ----------------------------------------
        # compute the CTE
 
        # start with the present robot estimate
        estimate = filter.get_position()
 
        # some basic vector calculations
        dx = spath[index+1][0] - spath[index][0]
        dy = spath[index+1][1] - spath[index][1]
        drx = estimate[0] - spath[index][0]
        dry = estimate[1] - spath[index][1]
        
        # u is the robot estimate projectes onto the path segment
        u = (drx * dx + dry * dy) / (dx * dx + dy * dy)
        
        # the cte is the estimate projected onto the normal of the path segment
        cte = (dry * dx - drx * dy) / (dx * dx + dy * dy)
        
        # pick the next path segment
        if u > 1.0 and index < len(spath) - 1:
            index += 1
        
 
        # ----------------------------------------
 
 
        diff_cte += cte
 
        steer = - params[0] * cte - params[1] * diff_cte 
 
        myrobot = myrobot.move(grid, steer, speed)
        filter.move(grid, steer, speed)
 
        Z = myrobot.sense()
        filter.sense(Z)
 
        if not myrobot.check_collision(grid):
            print '##### Collision ####'
 
        err += (cte ** 2)
        N += 1
 
        if printflag:
            print myrobot, cte, index, u
 
    return [myrobot.check_goal(goal), myrobot.num_collisions, myrobot.num_steps]
 
 
# ------------------------------------------------
# 
# this is our main routine
#
 
def main(grid, init, goal, steering_noise, distance_noise, measurement_noise, 
     weight_data, weight_smooth, p_gain, d_gain):
 
    path = plan(grid, init, goal)
    path.astar()
    path.smooth(weight_data, weight_smooth)
    return run(grid, goal, path.spath, [p_gain, d_gain])
 
    
 
 
# ------------------------------------------------
# 
# input data and parameters
#
 
 
# grid format:
#   0 = navigable space
#   1 = occupied space
 
grid = [[0, 1, 0, 0, 0, 0],
        [0, 1, 0, 1, 1, 0],
        [0, 1, 0, 1, 0, 0],
        [0, 0, 0, 1, 0, 1],
        [0, 1, 0, 1, 0, 0]]
 
 
init = [0, 0]
goal = [len(grid)-1, len(grid[0])-1]
 
 
steering_noise    = 0.1
distance_noise    = 0.03
measurement_noise = 0.3
 
#### ADJUST THESE PARAMETERS ######
 
weight_data       = 0.05
weight_smooth     = 0.15
p_gain            = 2.5
d_gain            = 6.5
 
###################################
    
print main(grid, init, goal, steering_noise, distance_noise, measurement_noise, 
           weight_data, weight_smooth, p_gain, d_gain)

 만약, 로봇이 가진 지도가 없어서 당장 자신이 어디 있는지도 모르다고 할 때, 알 수 있는 정보는 자신이 얼마나 움직였는지에 대한 벡터와, 지금 감지할 수 있는 물체들의 정보 뿐이다. 그런데 이들만 가지고로, 로봇은 충분히 지도를 만들어 낼 수 있다!! 다시 말하지만, 이것이 SLAM이고 그 중에서 가장 이해하기 쉬운 Graph SLAM에 대해서 배워 보려고 한다.

 

왼쪽 그림에서 로봇이 10만큼 움직였다고 했을 때, 실제로 정확히 가로로만 10만큼 움직여 주었다면 너무나 고맙겠지만, 고질적인 불확실성 때문에 이는 거의 불가하다. 그래서 우리는 지금까진 불확실성을 다루기 위해서 로봇의 위치를 가우시안 확률로 나타내었다. x-y좌표에 있으므로 각각 좌표에 하나씩 확률식을 세우고, 이들을 곱한다.

 우리가 하고 싶은 것은 주어진 x0 y0 값들과 움직인 양을 가지고 지금 로봇이 어디 있는지 알고 싶은 것이다. 계속 움직일 때 마다 가우시안들이 곱해질 것이고 그 식에서 최대 값을 가지는 곳을 찾는 것과 동일하다고 할 수 있다. Graph SLAM이 하는 일은 이 확률을 계속해서 새로고침하는 것이라 할 수 있다. 

 Graph SLAM을 짜면서 우리가 알고 있는 내용은 

1. 초기 위치

2. 직전 위치에서 얼마나 움직였는지(정확하진 않음)

3. Landmark들을 관찰함으로 인해서 알 수 있는 measurement 값들 

이것들을 이용해서 지금 자신이 어디에 있는지, 그리고 Landmark들이 어떤 위치에 있는지를 알아내는 것이 목표이다. 

다만, 지금의 예제에서는 Landmark 개수를 한정지어서 완벽한 Mapping을 한다고는 할 수 없겠다. 보통 visual SLAM을 하게 된다면, edge같은 feature들을 이용해서 보다 사실적인 지도를 만들고 이것이 일반적으로 SLAM이라고 했을 때 생각하는 이미지인데, 그 기초 과정이라고 할 수 있다.

 그럼 이 Constraint들을 실직적으로 어떻게 써먹는지 알아보자. 본질적으로 이들은 모두 '벡터'이다. 수없이 많은 벡터들이 계속해서 주어질 것이고, 추가가 될 때마다 이들을 일일이 계산하는 것도 성가실 테므로, 우리는 행렬을 사용하자!!

 위와 같이 모든 position과 landmark들을 추가한 거대한 행렬을 만들었다. 그리고 이제 조건들을 추가해줄 것인데, motion를 고려하는 것에 대해서 먼저 알아보자. x1 = x0 + 5라는 조건이 주어졌을 때, 이를 두 가지로 변형할 수 있다.

x1 - x0 = 5

x0 - x1 = -5

행렬을 만들면서 모든 변수를 추가했기 때문에 x1 - x0 = 5 이렇게만 추가해 주는 것이 아니라. x0의 관점에서도 변경해 주어야 한다. 그리고 오른편의 긴 column 벡터에 대해서도 5만 추가하는 것이 아니라 변형된 식의 -5를 고려해야 한다. 그래서 위 그림처럼 1 -1 , -1 1 이러한 모양을 가지게 되는 것이다. 이번에는 x1와 x2의 관계를 추가해 보자. 

 이렇게 할 수 있다. 여기서 알 수 있는 점은 행렬을 diagonal한 모양을 가지게 되고, 연속된 2가지 변수(x0 & x1 / x1 & x2)들에 대해서 조건을 추가했기 때문에, 정사각형 모양이 대각 방향으로 내려오면서 (x_t, x_t), (x_t, x_t+1) / (x_t+1, x_t), (x_t+1, x_t+1)에 1 -1 / -1 1이 더해진다고 볼 수 있다. value들에 대해서도 (x_t, x_t+1)에 -value / value이 이렇게 차례로 한칸씩 내려오면서 더해질 것이다. 사실 그림으로 설명하는 것이 좋은데, 그래서 동영상을 첨부한다. ㅠㅠ

 이번에는 Landmark들에 대한 조건을 추가해 보자!! x1에서 Landmark0가 관찰되었다고 했을 때, x1과 Landmark가 관여된 모든 요소들이 변경되어야 한다. 하나에 조건에 대해서 (x1, x1) (x1, L0) (L0, x1) (L0, L0)이 네 가지 요소들에 변화가 생긴다는 것이다. 

  

x1에서 관측된 L0가 9의 거리를 가지고 있었다고 한다면, x1 + 9 = L0라는 뜻이다. 그래서 이를 변형하여

x1 - L0 = -9

L0 - x1 = 9

이 두 식을 얻을 수 있을 것이고, 이제 행렬의 각 요소에 이에 해당되는 값들을 추가할 수 있다. 오른쪽 그림과 같이 말이다. Landmark를 measurement할 때마다, 그리고 어떠한 motion을 취할 때마다 행렬의 어떤 부분에 수정이 이루어 지는지 이제 알게 되었다. 그럼 아래 문제를 풀어 보자.

 선으로 연결된 부분은 우리가 조건을 알고 있다는 뜻이다. 결국 x0-x1 / x1-x2 / x0-L / x2-L이 네 가지 조건을 알고 있다고 했을 때, 행렬에서 0을 가지는 부분이 어디일까? 라는 문제였다. 사진에서 빨간 0이 있는 부분은 우리가 건들지 못하는 부분이다. 커다란 행렬을 사용하는데 거의 모든 부분을 건드린다는 걸 보여주고 싶었나 보다.

 그런데 우리가 이 행렬을 정의한 이유는 단순히 계산의 편의를 위해서만이 아니다. 정방 행렬을 Omega, 세로로 긴 행렬을 Xi라고 했을 때, Omega(inverse) * Xi를 하면 모든 요소들의 추정된 위치를 알 수 있게 된다!! 그럼 실제로 그러한지 예제를 통해서 알아보자. 

 예제는 위 경우를 가지고 진행할 것이다. x0가 3이었으니 결과는 -3 2 5 이러한 값이 나와야 할 텐데 실제로 그러한지 한 번 코드를 짜 보자는 것이다. 코드는 다음과 같다.

# -----------
# User Instructions
#
# Write a function, doit, that takes as its input an
# initial robot position, move1, and move2. This
# function should compute the Omega and Xi matrices
# discussed in lecture and should RETURN the mu vector
# (which is the product of Omega.inverse() and Xi).
#
# Please enter your code at the bottom.
 
 
 
from math import *
import random
 
 
#===============================================================
#
# SLAM in a rectolinear world (we avoid non-linearities)
#      
# 
#===============================================================
 
 
# ------------------------------------------------
# 
# this is the matrix class
# we use it because it makes it easier to collect constraints in GraphSLAM
# and to calculate solutions (albeit inefficiently)
# 
 
class matrix:
    
    # implements basic operations of a matrix class
 
    # ------------
    #
    # initialization - can be called with an initial matrix
    #
 
    def __init__(self, value = [[]]):
        self.value = value
        self.dimx  = len(value)
        self.dimy  = len(value[0])
        if value == [[]]:
            self.dimx = 0
 
    # ------------
    #
    # makes matrix of a certain size and sets each element to zero
    #
 
    def zero(self, dimx, dimy = 0):
        if dimy == 0:
            dimy = dimx
        # check if valid dimensions
        if dimx < 1 or dimy < 1:
            raise ValueError, "Invalid size of matrix"
        else:
            self.dimx  = dimx
            self.dimy  = dimy
            self.value = [[0.0 for row in range(dimy)] for col in range(dimx)]
 
    # ------------
    #
    # makes matrix of a certain (square) size and turns matrix into identity matrix
    #
 
 
    def identity(self, dim):
        # check if valid dimension
        if dim < 1:
            raise ValueError, "Invalid size of matrix"
        else:
            self.dimx  = dim
            self.dimy  = dim
            self.value = [[0.0 for row in range(dim)] for col in range(dim)]
            for i in range(dim):
                self.value[i][i] = 1.0
    # ------------
    #
    # prints out values of matrix
    #
 
 
    def show(self, txt = ''):
        for i in range(len(self.value)):
            print txt + '['+ ', '.join('%.3f'%x for x in self.value[i]) + ']' 
        print ' '
 
    # ------------
    #
    # defines elmement-wise matrix addition. Both matrices must be of equal dimensions
    #
 
 
    def __add__(self, other):
        # check if correct dimensions
        if self.dimx != other.dimx or self.dimy != other.dimy:
            raise ValueError, "Matrices must be of equal dimension to add"
        else:
            # add if correct dimensions
            res = matrix()
            res.zero(self.dimx, self.dimy)
            for i in range(self.dimx):
                for j in range(self.dimy):
                    res.value[i][j] = self.value[i][j] + other.value[i][j]
            return res
 
    # ------------
    #
    # defines elmement-wise matrix subtraction. Both matrices must be of equal dimensions
    #
 
    def __sub__(self, other):
        # check if correct dimensions
        if self.dimx != other.dimx or self.dimy != other.dimy:
            raise ValueError, "Matrices must be of equal dimension to subtract"
        else:
            # subtract if correct dimensions
            res = matrix()
            res.zero(self.dimx, self.dimy)
            for i in range(self.dimx):
                for j in range(self.dimy):
                    res.value[i][j] = self.value[i][j] - other.value[i][j]
            return res
 
    # ------------
    #
    # defines multiplication. Both matrices must be of fitting dimensions
    #
 
 
    def __mul__(self, other):
        # check if correct dimensions
        if self.dimy != other.dimx:
            raise ValueError, "Matrices must be m*n and n*p to multiply"
        else:
            # multiply if correct dimensions
            res = matrix()
            res.zero(self.dimx, other.dimy)
            for i in range(self.dimx):
                for j in range(other.dimy):
                    for k in range(self.dimy):
                        res.value[i][j] += self.value[i][k] * other.value[k][j]
        return res
 
 
    # ------------
    #
    # returns a matrix transpose
    #
 
 
    def transpose(self):
        # compute transpose
        res = matrix()
        res.zero(self.dimy, self.dimx)
        for i in range(self.dimx):
            for j in range(self.dimy):
                res.value[j][i] = self.value[i][j]
        return res
 
    # ------------
    #
    # creates a new matrix from the existing matrix elements.
    #
    # Example:
    #       l = matrix([[ 1,  2,  4,  3,  5], 
    #                   [ 6,  7,  8,  9, 10], 
    #                   [11, 12, 13, 14, 15]])
    #
    #       l.take([0, 2], [0, 2, 3])
    #
    # results in:
    #       
    #       [[1, 3, 4], 
    #        [11, 13, 14]]
    #       
    # 
    # take is used to remove rows and columns from existing matrices
    # list1/list2 define a sequence of rows/columns that shall be taken
    # is no list2 is provided, then list2 is set to list1 (good for symmetric matrices)
    #
 
    def take(self, list1, list2 = []):
        if list2 == []:
            list2 = list1
        if len(list1) > self.dimx or len(list2) > self.dimy:
            raise ValueError, "list invalid in take()"
 
        res = matrix()
        res.zero(len(list1), len(list2))
        for i in range(len(list1)):
            for j in range(len(list2)):
                res.value[i][j] = self.value[list1[i]][list2[j]]
        return res
 
    # ------------
    #
    # creates a new matrix from the existing matrix elements.
    #
    # Example:
    #       l = matrix([[1, 2, 3],
    #                  [4, 5, 6]])
    #
    #       l.expand(3, 5, [0, 2], [0, 2, 3])
    #
    # results in:
    #
    #       [[1, 0, 2, 3, 0], 
    #        [0, 0, 0, 0, 0], 
    #        [4, 0, 5, 6, 0]]
    # 
    # expand is used to introduce new rows and columns into an existing matrix
    # list1/list2 are the new indexes of row/columns in which the matrix
    # elements are being mapped. Elements for rows and columns 
    # that are not listed in list1/list2 
    # will be initialized by 0.0.
    #
 
    def expand(self, dimx, dimy, list1, list2 = []):
        if list2 == []:
            list2 = list1
        if len(list1) > self.dimx or len(list2) > self.dimy:
            raise ValueError, "list invalid in expand()"
 
        res = matrix()
        res.zero(dimx, dimy)
        for i in range(len(list1)):
            for j in range(len(list2)):
                res.value[list1[i]][list2[j]] = self.value[i][j]
        return res
 
    # ------------
    #
    # Computes the upper triangular Cholesky factorization of  
    # a positive definite matrix.
    # This code is based on http://adorio-research.org/wordpress/?p=4560
 
    def Cholesky(self, ztol= 1.0e-5):
        res = matrix()
        res.zero(self.dimx, self.dimx)
 
        for i in range(self.dimx):
            S = sum([(res.value[k][i])**2 for k in range(i)])
            d = self.value[i][i] - S
            if abs(d) < ztol:
                res.value[i][i] = 0.0
            else: 
                if d < 0.0:
                    raise ValueError, "Matrix not positive-definite"
                res.value[i][i] = sqrt(d)
            for j in range(i+1, self.dimx):
                S = sum([res.value[k][i] * res.value[k][j] for k in range(i)])
                if abs(S) < ztol:
                    S = 0.0
                try:
                   res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
                except:
                   raise ValueError, "Zero diagonal"
        return res 
 
    # ------------
    #
    # Computes inverse of matrix given its Cholesky upper Triangular
    # decomposition of matrix.
    # This code is based on http://adorio-research.org/wordpress/?p=4560
 
    def CholeskyInverse(self):
        res = matrix()
        res.zero(self.dimx, self.dimx)
 
    # Backward step for inverse.
        for j in reversed(range(self.dimx)):
            tjj = self.value[j][j]
            S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
            res.value[j][j] = 1.0/ tjj**2 - S/ tjj
            for i in reversed(range(j)):
                res.value[j][i] = res.value[i][j] = \
                    -sum([self.value[i][k]*res.value[k][j] for k in \
                              range(i+1,self.dimx)])/self.value[i][i]
        return res
    
    # ------------
    #
    # comutes and returns the inverse of a square matrix
    #
 
 
    def inverse(self):
        aux = self.Cholesky()
        res = aux.CholeskyInverse()
        return res
 
    # ------------
    #
    # prints matrix (needs work!)
    #
 
 
    def __repr__(self):
        return repr(self.value)
 
 
 
# ######################################################################
# ######################################################################
# ######################################################################
 
 
"""
For the following example, you would call doit(-3, 5, 3):
3 robot positions
  initially: -3
  moves by 5
  moves by 3

  

which should return a mu of:
[[-3.0],
 [2.0],
 [5.0]]
"""
 
def doit(initial_pos, move1, move2):
    Omega = matrix([[1, 0, 0], [0, 0, 0], [0, 0, 0]])
    xi = matrix([[initial_pos], [0], [0]])
    Omega += matrix([[1, -1, 0], [-1, 1, 0], [0, 0, 0]])
    xi += matrix([[-move1], [move1], [0]])
    Omega += matrix([[0, 0, 0], [0, 1, -1], [0, -1, 1]])
    xi += matrix([[0], [-move2], [move2]])
 
    #Omega.show()
    #xi.show()
    mu = Omega.inverse() * xi
    #mu.show()
    return mu
 
doit(-3, 5, 3)

def doit(initial_pos, move1, move2):

    Omega = matrix([[1, 0, 0], [0, 0, 0], [0, 0, 0]])

    xi = matrix([[initial_pos], [0], [0]])

    Omega += matrix([[1, -1, 0], [-1, 1, 0], [0, 0, 0]])

    xi += matrix([[-move1], [move1], [0]])

    Omega += matrix([[0, 0, 0], [0, 1, -1], [0, -1, 1]])

    xi += matrix([[0], [-move2], [move2]])

 

    #Omega.show()

    #xi.show()

    mu = Omega.inverse() * xi

    #mu.show()

    return mu

 

doit(-3, 5, 3)

 위 부분이 핵심인데, 앞서 설명했으므로 한 번 읽어보면 이해가 갈 것이다. 

이번에는 그럼 Landmark를 추가해 보자! 

 로봇의 처음 위치는 -3이었고 Landmark는 10만큼 떨어져 있었다고 한다.

 로봇이 5만큼 움직였고 이 위치에서 Landmark는 5만큼 떨어져 있었다고 한다.

 로봇이 또 3만큼 움직였고 이번에는 Landmark가 2만큼 떨어져 있었다고 한다.

 그럼 이 상황에서 Landmark의 위치는??

 -3 + 10 = -3 + 5 + 5 = -3 + 5 + 3 + 2 = 7

 모든 정황들이 Landmark의 위치가 7이라고 말하고 있다. 왜 같은 말을 이렇게 여러번 하는지 이해를 못할 수도 있다. 그런데, 실제로는 noise가 있어서 같은 값이 계속 나오는 것이 아니다. 나중에 볼 것이다. 본론으로 돌아가서, 우리가 만들고 있는 행렬에 이러한 요소를 추가시켜 준다면, 위와 같은 여러 번의 연산을 일일이 짜주지 않아도 행렬의 역행렬 연산을 통해서 손쉽게 Landmark를 추정할 수 있다. 그럼 이제 Landmark 요소를 추가시켜 보자!!!

def doit(initial_pos, move1, move2, Z0, Z1, Z2):

    Omega = matrix([[1.0, 0.0, 0.0],

                    [0.0, 0.0, 0.0],

                    [0.0, 0.0, 0.0]])

                    

    Xi = matrix([[initial_pos],

                 [0.0],

                 [0.0]])

 

    Omega += matrix([[1.0, -1.0, 0.0],

                     [-1.0, 1.0, 0.0],

                     [0.0, 0.0, 0.0]])

    Xi += matrix([[-move1],

                  [move1],

                  [0.0]])

    

    Omega += matrix([[0.0, 0.0, 0.0],

                     [0.0, 1.0, -1.0],

                     [0.0, -1.0, 1.0]])

    Xi += matrix([[0.0],

                  [-move2],

                  [move2]])

 

    Omega = Omega.expand(4, 4, [0, 1, 2], [0, 1, 2])

    Xi = Xi.expand(4, 1, [0, 1, 2], [0])

 

    Omega += matrix([[1, 0, 0, -1],

                     [0, 0, 0, 0],

                     [0, 0, 0, 0],

                     [-1, 0, 0, 1]])

    Xi += matrix([[-10],

                  [0],

                  [0],

                  [10]])

    Omega += matrix([[0, 0, 0, 0],

                     [0, 1, 0, -1],

                     [0, 0, 0, 0],

                     [0, -1, 0, 1]])

    Xi += matrix([[0],

                  [-5],

                  [0],

                  [5]])

    Omega += matrix([[0, 0, 0, 0],

                     [0, 0, 0, 0],

                     [0, 0, 1, -1],

                     [0, 0, -1, 1]])

    Xi += matrix([[0],

                  [0],

                  [-2],

                  [2]])

 

 

    Omega.show('Omega: ')

    Xi.show('Xi:    ')

    mu = Omega.inverse() * Xi

    mu.show('Mu:    ')

    

    return mu

 갑자기 코드가 나왔지만 실제로 볼 부분은 아래이다. 

    Omega = matrix([[1.0, 0.0, 0.0],

                    [0.0, 0.0, 0.0],

                    [0.0, 0.0, 0.0]])

                    

    Xi = matrix([[initial_pos],

                 [0.0],

                 [0.0]])

 

    Omega += matrix([[1.0, -1.0, 0.0],

                     [-1.0, 1.0, 0.0],

                     [0.0, 0.0, 0.0]])

    Xi += matrix([[-move1],

                  [move1],

                  [0.0]])

    

    Omega += matrix([[0.0, 0.0, 0.0],

                     [0.0, 1.0, -1.0],

                     [0.0, -1.0, 1.0]])

    Xi += matrix([[0.0],

                  [-move2],

                  [move2]])

 여기까지는 위에서 했었다. 이 때는 Landmark는 생각도 하지 않고 motion만 고려했었는데 그러다 보니 행렬의 크기가 커져야 함을 알 수 있다. 그래서 expand라는 함수를 사용해서 행렬의 크기를 키워 주었다.

    Omega = Omega.expand(4, 4, [0, 1, 2], [0, 1, 2])

    Xi = Xi.expand(4, 1, [0, 1, 2], [0])

그리고 이제 Landmark 요소들을 추가해 줄 것이다. 위 그림을 보면 x0에서 10만큼 떨어져 있었는데 이를 식으로 나타내면 x0 + 10 = L 이렇게 된다. 이를 motion에서 해 주었던 것과 똑같이 x0 - L = -10 / L - x0 = 10 이렇게 바꾸어서 행렬에 추가할 것이다. 그 부분이 아래이다.

    Omega += matrix([[1, 0, 0, -1],

                     [0, 0, 0, 0],

                     [0, 0, 0, 0],

                     [-1, 0, 0, 1]])

    Xi += matrix([[-10],

                  [0],

                  [0],

                  [10]])

    Omega += matrix([[0, 0, 0, 0],

                     [0, 1, 0, -1],

                     [0, 0, 0, 0],

                     [0, -1, 0, 1]])

    Xi += matrix([[0],

                  [-5],

                  [0],

                  [5]])

    Omega += matrix([[0, 0, 0, 0],

                     [0, 0, 0, 0],

                     [0, 0, 1, -1],

                     [0, 0, -1, 1]])

    Xi += matrix([[0],

                  [0],

                  [-2],

                  [2]])

 

 원시적인 Graph SLAM이 완성되었다!! 이 원리만 알고 있으면 좌표계를 확장하고 더 많은 움직임, 더 많은 Landmark들을 추가만 해 주는 것은 단순 작업일 뿐이다. 

 앞서 살짝 이야기했는데 여러 위치에서 Landmark가 보일 수는 있지만, 추정된 Landmark의 위치들은 다를 수 있다. 이번에는 이 noise를 추가해 보는 작업을 할 것이다. 

 measurement의 불확실성에 대한 이야기이다. 우리가 measurement를 할 때마다 위의 가우시안 폼의 수식들이 계속해서 곱해지게 된다. 그런데, 이 과정을 수행하면서, 모든 measurement를 똑같이 신뢰하기보다. 어떤 특정한measurement 값은 확신을 가지고 있다고 하자, 그랬을 때, 해당하는 value들에 weight(수학적으로 더 이야기하자면 1/sigma)를 곱해줌으로서 이에 해당하는 결과를 얻을 수 있게 된다. 여기서는 *5를 해주기로 했다. 코드로 나타내면 아래와 같다. 

    Omega += matrix([[0.0, 0.0, 0.0, 0.0],
                     [0.0, 0.0, 0.0, 0.0],
                     [0.0, 0.0, 5.0, -5.0],
                     [0.0, 0.0, -5.0, 5.0]])
    Xi    += matrix([[0.0],
                     [0.0],
                     [-Z2 * 5.0],
                     [Z2 * 5.0]])

필요한 부분만 추가를 했다. 그럼 이번에는 일반적인 경우로 확장을 해서 임의의 data가 들어왔을 때, 2-D SLAM 구현하는 코드를 보이겠다. data는 이렇게 들어온다. 

test_data1 = [[[[1, 19.457599255548065, 23.8387362100849], [2, -13.195807561967236, 11.708840328458608], [3, -30.0954905279171, 15.387879242505843]], [-12.2607279422326, -15.801093326936487]], [[[2, -0.4659930049620491, 28.088559771215664], [4, -17.866382374890936, -16.384904503932]], [-12.2607279422326, -15.801093326936487]], [[[4, -6.202512900833806, -1.823403210274639]], [-12.2607279422326, -15.801093326936487]], [[[4, 7.412136480918645, 15.388585962142429]], [14.008259661173426, 14.274756084260822]], [[[4, -7.526138813444998, -0.4563942429717849]], [14.008259661173426, 14.274756084260822]], [[[2, -6.299793150150058, 29.047830407717623], [4, -21.93551130411791, -13.21956810989039]], [14.008259661173426, 14.274756084260822]], [[[1, 15.796300959032276, 30.65769689694247], [2, -18.64370821983482, 17.380022987031367]], [14.008259661173426, 14.274756084260822]], [[[1, 0.40311325410337906, 14.169429532679855], [2, -35.069349468466235, 2.4945558982439957]], [14.008259661173426, 14.274756084260822]], [[[1, -16.71340983241936, -2.777000269543834]], [-11.006096015782283, 16.699276945166858]], [[[1, -3.611096830835776, -17.954019226763958]], [-19.693482634035977, 3.488085684573048]], [[[1, 18.398273354362416, -22.705102332550947]], [-19.693482634035977, 3.488085684573048]], [[[2, 2.789312482883833, -39.73720193121324]], [12.849049222879723, -15.326510824972983]], [[[1, 21.26897046581808, -10.121029799040915], [2, -11.917698965880655, -23.17711662602097], [3, -31.81167947898398, -16.7985673023331]], [12.849049222879723, -15.326510824972983]], [[[1, 10.48157743234859, 5.692957082575485], [2, -22.31488473554935, -5.389184118551409], [3, -40.81803984305378, -2.4703329790238118]], [12.849049222879723, -15.326510824972983]], [[[0, 10.591050242096598, -39.2051798967113], [1, -3.5675572049297553, 22.849456408289125], [2, -38.39251065320351, 7.288990306029511]], [12.849049222879723, -15.326510824972983]], [[[0, -3.6225556479370766, -25.58006865235512]], [-7.8874682868419965, -18.379005523261092]], [[[0, 1.9784503557879374, -6.5025974151499]], [-7.8874682868419965, -18.379005523261092]], [[[0, 10.050665232782423, 11.026385307998742]], [-17.82919359778298, 9.062000642947142]], [[[0, 26.526838150174818, -0.22563393232425621], [4, -33.70303936886652, 2.880339841013677]], [-17.82919359778298, 9.062000642947142]]]
test_data2 = [[[[0, 26.543274387283322, -6.262538160312672], [3, 9.937396825799755, -9.128540360867689]], [18.92765331253674, -6.460955043986683]], [[[0, 7.706544739722961, -3.758467215445748], [1, 17.03954411948937, 31.705489938553438], [3, -11.61731288777497, -6.64964096716416]], [18.92765331253674, -6.460955043986683]], [[[0, -12.35130507136378, 2.585119104239249], [1, -2.563534536165313, 38.22159657838369], [3, -26.961236804740935, -0.4802312626141525]], [-11.167066095509824, 16.592065417497455]], [[[0, 1.4138633151721272, -13.912454837810632], [1, 8.087721200818589, 20.51845934354381], [3, -17.091723454402302, -16.521500551709707], [4, -7.414211721400232, 38.09191602674439]], [-11.167066095509824, 16.592065417497455]], [[[0, 12.886743222179561, -28.703968411636318], [1, 21.660953298391387, 3.4912891084614914], [3, -6.401401414569506, -32.321583037341625], [4, 5.034079343639034, 23.102207946092893]], [-11.167066095509824, 16.592065417497455]], [[[1, 31.126317672358578, -10.036784369535214], [2, -38.70878528420893, 7.4987265861424595], [4, 17.977218575473767, 6.150889254289742]], [-6.595520680493778, -18.88118393939265]], [[[1, 41.82460922922086, 7.847527392202475], [3, 15.711709540417502, -30.34633659912818]], [-6.595520680493778, -18.88118393939265]], [[[0, 40.18454208294434, -6.710999804403755], [3, 23.019508919299156, -10.12110867290604]], [-6.595520680493778, -18.88118393939265]], [[[3, 27.18579315312821, 8.067219022708391]], [-6.595520680493778, -18.88118393939265]], [[], [11.492663265706092, 16.36822198838621]], [[[3, 24.57154567653098, 13.461499960708197]], [11.492663265706092, 16.36822198838621]], [[[0, 31.61945290413707, 0.4272295085799329], [3, 16.97392299158991, -5.274596836133088]], [11.492663265706092, 16.36822198838621]], [[[0, 22.407381798735177, -18.03500068379259], [1, 29.642444125196995, 17.3794951934614], [3, 4.7969752441371645, -21.07505361639969], [4, 14.726069092569372, 32.75999422300078]], [11.492663265706092, 16.36822198838621]], [[[0, 10.705527984670137, -34.589764174299596], [1, 18.58772336795603, -0.20109708164787765], [3, -4.839806195049413, -39.92208742305105], [4, 4.18824810165454, 14.146847823548889]], [11.492663265706092, 16.36822198838621]], [[[1, 5.878492140223764, -19.955352450942357], [4, -7.059505455306587, -0.9740849280550585]], [19.628527845173146, 3.83678180657467]], [[[1, -11.150789592446378, -22.736641053247872], [4, -28.832815721158255, -3.9462962046291388]], [-19.841703647091965, 2.5113335861604362]], [[[1, 8.64427397916182, -20.286336970889053], [4, -5.036917727942285, -6.311739993868336]], [-5.946642674882207, -19.09548221169787]], [[[0, 7.151866679283043, -39.56103232616369], [1, 16.01535401373368, -3.780995345194027], [4, -3.04801331832137, 13.697362774960865]], [-5.946642674882207, -19.09548221169787]], [[[0, 12.872879480504395, -19.707592098123207], [1, 22.236710716903136, 16.331770792606406], [3, -4.841206109583004, -21.24604435851242], [4, 4.27111163223552, 32.25309748614184]], [-5.946642674882207, -19.09548221169787]]] 
 

 data는 위와 같은 형식으로 구성되어 있다.

1. 로봇이 모든 Landmark들을 볼 수는 없다고 가정을 한다. (볼 수 있는 range가 있다.) measurement를 먼저 하고 motion을 취한다고 가정한다.

2. 1, 2, 3, 4 이렇게 Landmark들에는 이름이 붙어 있으며(사실상 이 부분에서 완벽한 SLAM은 아니라고 생각을 한다.),  볼 수 있는 Landmark들 까지의 x-y 거리가 data로 들어온다.

3. Landmark들을 한 위치에서 다수 볼 수 있기에 data가 주르륵 들어오는데, 마지막으로는 자기가 움직인 x-y 거리가 data로 들어온다.

4. 계속 움직이면서 계속계속 이런 data들을 받는다. 

 이러한 상황에서 모든 요소를 포함하고 있는 행렬들을 만들고 이들의 연산을 통해서 모든 position을 estimate하고 Landmark들의 위치도 estimate하라!!! 아래는 정답이다. 

# ------------
# User Instructions
# 
# In this problem you will implement SLAM in a 2 dimensional
# world. Please define a function, slam, which takes five
# parameters as input and returns the vector mu. This vector
# should have x, y coordinates interlaced, so for example, 
# if there were 2 poses and 2 landmarks, mu would look like:
#
#  mu =  matrix([[Px0],
#                [Py0],
#                [Px1],
#                [Py1],
#                [Lx0],
#                [Ly0],
#                [Lx1],
#                [Ly1]])
#
# data - This is the data that is generated with the included
#        make_data function. You can also use test_data to
#        make sure your function gives the correct result.
#
# N -    The number of time steps.
#
# num_landmarks - The number of landmarks.
#
# motion_noise - The noise associated with motion. The update
#                strength for motion should be 1.0 / motion_noise.
#
# measurement_noise - The noise associated with measurement.
#                     The update strength for measurement should be
#                     1.0 / measurement_noise.
#
#
# Enter your code at line 509
 
# --------------
# Testing
#
# Uncomment the test cases at the bottom of this document.
# Your output should be identical to the given results.
 
 
from math import *
import random
 
 
#===============================================================
#
# SLAM in a rectolinear world (we avoid non-linearities)
#      
# 
#===============================================================
 
 
# ------------------------------------------------
# 
# this is the matrix class
# we use it because it makes it easier to collect constraints in GraphSLAM
# and to calculate solutions (albeit inefficiently)
# 
 
class matrix:
    
    # implements basic operations of a matrix class
 
    # ------------
    #
    # initialization - can be called with an initial matrix
    #
 
    def __init__(self, value = [[]]):
        self.value = value
        self.dimx  = len(value)
        self.dimy  = len(value[0])
        if value == [[]]:
            self.dimx = 0
 
    # ------------
    #
    # makes matrix of a certain size and sets each element to zero
    #
 
    def zero(self, dimx, dimy):
        if dimy == 0:
            dimy = dimx
        # check if valid dimensions
        if dimx < 1 or dimy < 1:
            raise ValueError, "Invalid size of matrix"
        else:
            self.dimx  = dimx
            self.dimy  = dimy
            self.value = [[0.0 for row in range(dimy)] for col in range(dimx)]
 
    # ------------
    #
    # makes matrix of a certain (square) size and turns matrix into identity matrix
    #
 
    def identity(self, dim):
        # check if valid dimension
        if dim < 1:
            raise ValueError, "Invalid size of matrix"
        else:
            self.dimx  = dim
            self.dimy  = dim
            self.value = [[0.0 for row in range(dim)] for col in range(dim)]
            for i in range(dim):
                self.value[i][i] = 1.0
    # ------------
    #
    # prints out values of matrix
    #
 
    def show(self, txt = ''):
        for i in range(len(self.value)):
            print txt + '['+ ', '.join('%.3f'%x for x in self.value[i]) + ']' 
        print ' '
 
    # ------------
    #
    # defines elmement-wise matrix addition. Both matrices must be of equal dimensions
    #
 
    def __add__(self, other):
        # check if correct dimensions
        if self.dimx != other.dimx or self.dimy != other.dimy:
            raise ValueError, "Matrices must be of equal dimension to add"
        else:
            # add if correct dimensions
            res = matrix()
            res.zero(self.dimx, self.dimy)
            for i in range(self.dimx):
                for j in range(self.dimy):
                    res.value[i][j] = self.value[i][j] + other.value[i][j]
            return res
 
    # ------------
    #
    # defines elmement-wise matrix subtraction. Both matrices must be of equal dimensions
    #
 
    def __sub__(self, other):
        # check if correct dimensions
        if self.dimx != other.dimx or self.dimy != other.dimy:
            raise ValueError, "Matrices must be of equal dimension to subtract"
        else:
            # subtract if correct dimensions
            res = matrix()
            res.zero(self.dimx, self.dimy)
            for i in range(self.dimx):
                for j in range(self.dimy):
                    res.value[i][j] = self.value[i][j] - other.value[i][j]
            return res
 
    # ------------
    #
    # defines multiplication. Both matrices must be of fitting dimensions
    #
 
    def __mul__(self, other):
        # check if correct dimensions
        if self.dimy != other.dimx:
            raise ValueError, "Matrices must be m*n and n*p to multiply"
        else:
            # multiply if correct dimensions
            res = matrix()
            res.zero(self.dimx, other.dimy)
            for i in range(self.dimx):
                for j in range(other.dimy):
                    for k in range(self.dimy):
                        res.value[i][j] += self.value[i][k] * other.value[k][j]
        return res
 
 
    # ------------
    #
    # returns a matrix transpose
    #
 
    def transpose(self):
        # compute transpose
        res = matrix()
        res.zero(self.dimy, self.dimx)
        for i in range(self.dimx):
            for j in range(self.dimy):
                res.value[j][i] = self.value[i][j]
        return res
 
    # ------------
    #
    # creates a new matrix from the existing matrix elements.
    #
    # Example:
    #       l = matrix([[ 1,  2,  3,  4,  5], 
    #                   [ 6,  7,  8,  9, 10], 
    #                   [11, 12, 13, 14, 15]])
    #
    #       l.take([0, 2], [0, 2, 3])
    #
    # results in:
    #       
    #       [[1, 3, 4], 
    #        [11, 13, 14]]
    #       
    # 
    # take is used to remove rows and columns from existing matrices
    # list1/list2 define a sequence of rows/columns that shall be taken
    # if no list2 is provided, then list2 is set to list1 (good for 
    # symmetric matrices)
    #
 
    def take(self, list1, list2 = []):
        if list2 == []:
            list2 = list1
        if len(list1) > self.dimx or len(list2) > self.dimy:
            raise ValueError, "list invalid in take()"
 
        res = matrix()
        res.zero(len(list1), len(list2))
        for i in range(len(list1)):
            for j in range(len(list2)):
                res.value[i][j] = self.value[list1[i]][list2[j]]
        return res
 
    # ------------
    #
    # creates a new matrix from the existing matrix elements.
    #
    # Example:
    #       l = matrix([[1, 2, 3],
    #                  [4, 5, 6]])
    #
    #       l.expand(3, 5, [0, 2], [0, 2, 3])
    #
    # results in:
    #
    #       [[1, 0, 2, 3, 0], 
    #        [0, 0, 0, 0, 0], 
    #        [4, 0, 5, 6, 0]]
    # 
    # expand is used to introduce new rows and columns into an existing matrix
    # list1/list2 are the new indexes of row/columns in which the matrix
    # elements are being mapped. Elements for rows and columns 
    # that are not listed in list1/list2 
    # will be initialized by 0.0.
    #
    
    def expand(self, dimx, dimy, list1, list2 = []):
        if list2 == []:
            list2 = list1
        if len(list1) > self.dimx or len(list2) > self.dimy:
            raise ValueError, "list invalid in expand()"
 
        res = matrix()
        res.zero(dimx, dimy)
        for i in range(len(list1)):
            for j in range(len(list2)):
                res.value[list1[i]][list2[j]] = self.value[i][j]
        return res
 
    # ------------
    #
    # Computes the upper triangular Cholesky factorization of  
    # a positive definite matrix.
    # This code is based on http://adorio-research.org/wordpress/?p=4560
    #
    
    def Cholesky(self, ztol= 1.0e-5):
 
        res = matrix()
        res.zero(self.dimx, self.dimx)
 
        for i in range(self.dimx):
            S = sum([(res.value[k][i])**2 for k in range(i)])
            d = self.value[i][i] - S
            if abs(d) < ztol:
                res.value[i][i] = 0.0
            else: 
                if d < 0.0:
                    raise ValueError, "Matrix not positive-definite"
                res.value[i][i] = sqrt(d)
            for j in range(i+1, self.dimx):
                S = sum([res.value[k][i] * res.value[k][j] for k in range(i)])
                if abs(S) < ztol:
                    S = 0.0
                try:
                   res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
                except:
                   raise ValueError, "Zero diagonal"
        return res 
 
    # ------------
    #
    # Computes inverse of matrix given its Cholesky upper Triangular
    # decomposition of matrix.
    # This code is based on http://adorio-research.org/wordpress/?p=4560
    #
    
    def CholeskyInverse(self):
 
        res = matrix()
        res.zero(self.dimx, self.dimx)
 
        # Backward step for inverse.
        for j in reversed(range(self.dimx)):
            tjj = self.value[j][j]
            S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
            res.value[j][j] = 1.0/ tjj**2 - S/ tjj
            for i in reversed(range(j)):
                res.value[j][i] = res.value[i][j] = \
                    -sum([self.value[i][k]*res.value[k][j] for k in \
                              range(i+1,self.dimx)])/self.value[i][i]
        return res
    
    # ------------
    #
    # computes and returns the inverse of a square matrix
    #
    def inverse(self):
        aux = self.Cholesky()
        res = aux.CholeskyInverse()
        return res
 
    # ------------
    #
    # prints matrix (needs work!)
    #
    def __repr__(self):
        return repr(self.value)
 
# ------------------------------------------------
# 
# this is the robot class
# 
# our robot lives in x-y space, and its motion is
# pointed in a random direction. It moves on a straight line
# until is comes close to a wall at which point it turns
# away from the wall and continues to move.
#
# For measurements, it simply senses the x- and y-distance
# to landmarks. This is different from range and bearing as 
# commonly studied in the literature, but this makes it much
# easier to implement the essentials of SLAM without
# cluttered math
#
 
class robot:
 
    # --------
    # init: 
    #   creates robot and initializes location to 0, 0
    #
 
    def __init__(self, world_size = 100.0, measurement_range = 30.0,
                 motion_noise = 1.0, measurement_noise = 1.0):
        self.measurement_noise = 0.0
        self.world_size = world_size
        self.measurement_range = measurement_range
        self.x = world_size / 2.0
        self.y = world_size / 2.0
        self.motion_noise = motion_noise
        self.measurement_noise = measurement_noise
        self.landmarks = []
        self.num_landmarks = 0
 
 
    def rand(self):
        return random.random() * 2.0 - 1.0
 
    # --------
    #
    # make random landmarks located in the world
    #
 
    def make_landmarks(self, num_landmarks):
        self.landmarks = []
        for i in range(num_landmarks):
            self.landmarks.append([round(random.random() * self.world_size),
                                   round(random.random() * self.world_size)])
        self.num_landmarks = num_landmarks
 
 
    # --------
    #
    # move: attempts to move robot by dx, dy. If outside world
    #       boundary, then the move does nothing and instead returns failure
    #
 
    def move(self, dx, dy):
 
        x = self.x + dx + self.rand() * self.motion_noise
        y = self.y + dy + self.rand() * self.motion_noise
 
        if x < 0.0 or x > self.world_size or y < 0.0 or y > self.world_size:
            return False
        else:
            self.x = x
            self.y = y
            return True
    
 
    # --------
    #
    # sense: returns x- and y- distances to landmarks within visibility range
    #        because not all landmarks may be in this range, the list of measurements
    #        is of variable length. Set measurement_range to -1 if you want all
    #        landmarks to be visible at all times
    #
 
    def sense(self):
        Z = []
        for i in range(self.num_landmarks):
            dx = self.landmarks[i][0] - self.x + self.rand() * self.measurement_noise
            dy = self.landmarks[i][1] - self.y + self.rand() * self.measurement_noise    
            if self.measurement_range < 0.0 or abs(dx) + abs(dy) <= self.measurement_range:
                Z.append([i, dx, dy])
        return Z
 
    # --------
    #
    # print robot location
    #
 
    def __repr__(self):
        return 'Robot: [x=%.5f y=%.5f]'  % (self.x, self.y)
 
######################################################
 
# --------
# this routine makes the robot data
#
 
def make_data(N, num_landmarks, world_size, measurement_range, motion_noise, 
              measurement_noise, distance):
 
 
    complete = False
 
    while not complete:
 
        data = []
 
        # make robot and landmarks
        r = robot(world_size, measurement_range, motion_noise, measurement_noise)
        r.make_landmarks(num_landmarks)
        seen = [False for row in range(num_landmarks)]
    
        # guess an initial motion
        orientation = random.random() * 2.0 * pi
        dx = cos(orientation) * distance
        dy = sin(orientation) * distance
    
        for k in range(N-1):
    
            # sense
            Z = r.sense()
 
            # check off all landmarks that were observed 
            for i in range(len(Z)):
                seen[Z[i][0]] = True
    
            # move
            while not r.move(dx, dy):
                # if we'd be leaving the robot world, pick instead a new direction
                orientation = random.random() * 2.0 * pi
                dx = cos(orientation) * distance
                dy = sin(orientation) * distance
 
            # memorize data
            data.append([Z, [dx, dy]])
 
        # we are done when all landmarks were observed; otherwise re-run
        complete = (sum(seen) == num_landmarks)
 
    print ' '
    print 'Landmarks: ', r.landmarks
    print r
 
 
    return data
    
####################################################
 
# --------------------------------
#
# print the result of SLAM, the robot pose(s) and the landmarks
#
 
def print_result(N, num_landmarks, result):
    print
    print 'Estimated Pose(s):'
    for i in range(N):
        print '    ['+ ', '.join('%.3f'%x for x in result.value[2*i]) + ', ' \
            + ', '.join('%.3f'%x for x in result.value[2*i+1]) +']'
    print
    print 'Estimated Landmarks:'
    for i in range(num_landmarks):
        print '    ['+ ', '.join('%.3f'%x for x in result.value[2*(N+i)]) + ', ' \
            + ', '.join('%.3f'%x for x in result.value[2*(N+i)+1]) +']'
 
# --------------------------------
#
# slam - retains entire path and all landmarks
#
 
############## ENTER YOUR CODE BELOW HERE ###################
 
def slam(data, N, num_landmarks, motion_noise, measurement_noise):
    dim = 2 * (N + num_landmarks)
    #print('dim', dim)
    
    Omega = matrix()
    Omega.zero(dim, dim)
    Omega.value[0][0] = 1.0
    Omega.value[1][1] = 1.0
    
    Xi = matrix()
    Xi.zero(dim, 1)
    Xi.value[0][0] = world_size / 2.0
    Xi.value[1][0] = world_size / 2.0
 
    #print('len',len(data))
    for k in range(len(data)):
        n = k * 2
 
        measurement = data[k][0]
        motion      = data[k][1]
        #print('measurement', measurement,'motion', motion)
        for i in range(len(measurement)):
            m = 2 * (N + measurement[i][0])
 
            for b in range(2):
                Omega.value[n+b][n+b] += 1.0 / measurement_noise
                Omega.value[n+b][m+b] += -1.0 / measurement_noise
                Omega.value[m+b][n+b] += -1.0 / measurement_noise
                Omega.value[m+b][m+b] += 1.0 / measurement_noise
                Xi.value[n+b][0]      += -measurement[i][b+1] / measurement_noise
                Xi.value[m+b][0]      +=  measurement[i][b+1] / measurement_noise
 
        for b in range(4):
            Omega.value[n+b][n+b] += 1.0 / motion_noise
        for b in range(2):
            Omega.value[n+b][n+b+2] += -1.0 / motion_noise
            Omega.value[n+b+2][n+b] += -1.0 / motion_noise
            Xi.value[n+b][0]           += -motion[b] / motion_noise
            Xi.value[n+b+2][0]         += motion[b] / motion_noise
            
    mu = Omega.inverse() * Xi
 
    return mu # Make sure you return mu for grading!
        
############### ENTER YOUR CODE ABOVE HERE ###################
 
# ------------------------------------------------------------------------
# ------------------------------------------------------------------------
# ------------------------------------------------------------------------
#
# Main routines
#
 
 
num_landmarks      = 5        # number of landmarks
N                  = 20       # time steps
world_size         = 100.0    # size of world
measurement_range  = 50.0     # range at which we can sense landmarks
motion_noise       = 2.0      # noise in robot motion
measurement_noise  = 2.0      # noise in the measurements
distance           = 20.0     # distance by which robot (intends to) move each iteratation 
 
data = make_data(N, num_landmarks, world_size, measurement_range, motion_noise, measurement_noise, distance)
result = slam(data, N, num_landmarks, motion_noise, measurement_noise)                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                
print_result(N, num_landmarks, result)
 
# -------------
# Testing
#
# Uncomment one of the test cases below to compare your results to
# the results shown for Test Case 1 and Test Case 2.
 
test_data1 = [[[[1, 19.457599255548065, 23.8387362100849], [2, -13.195807561967236, 11.708840328458608], [3, -30.0954905279171, 15.387879242505843]], [-12.2607279422326, -15.801093326936487]], [[[2, -0.4659930049620491, 28.088559771215664], [4, -17.866382374890936, -16.384904503932]], [-12.2607279422326, -15.801093326936487]], [[[4, -6.202512900833806, -1.823403210274639]], [-12.2607279422326, -15.801093326936487]], [[[4, 7.412136480918645, 15.388585962142429]], [14.008259661173426, 14.274756084260822]], [[[4, -7.526138813444998, -0.4563942429717849]], [14.008259661173426, 14.274756084260822]], [[[2, -6.299793150150058, 29.047830407717623], [4, -21.93551130411791, -13.21956810989039]], [14.008259661173426, 14.274756084260822]], [[[1, 15.796300959032276, 30.65769689694247], [2, -18.64370821983482, 17.380022987031367]], [14.008259661173426, 14.274756084260822]], [[[1, 0.40311325410337906, 14.169429532679855], [2, -35.069349468466235, 2.4945558982439957]], [14.008259661173426, 14.274756084260822]], [[[1, -16.71340983241936, -2.777000269543834]], [-11.006096015782283, 16.699276945166858]], [[[1, -3.611096830835776, -17.954019226763958]], [-19.693482634035977, 3.488085684573048]], [[[1, 18.398273354362416, -22.705102332550947]], [-19.693482634035977, 3.488085684573048]], [[[2, 2.789312482883833, -39.73720193121324]], [12.849049222879723, -15.326510824972983]], [[[1, 21.26897046581808, -10.121029799040915], [2, -11.917698965880655, -23.17711662602097], [3, -31.81167947898398, -16.7985673023331]], [12.849049222879723, -15.326510824972983]], [[[1, 10.48157743234859, 5.692957082575485], [2, -22.31488473554935, -5.389184118551409], [3, -40.81803984305378, -2.4703329790238118]], [12.849049222879723, -15.326510824972983]], [[[0, 10.591050242096598, -39.2051798967113], [1, -3.5675572049297553, 22.849456408289125], [2, -38.39251065320351, 7.288990306029511]], [12.849049222879723, -15.326510824972983]], [[[0, -3.6225556479370766, -25.58006865235512]], [-7.8874682868419965, -18.379005523261092]], [[[0, 1.9784503557879374, -6.5025974151499]], [-7.8874682868419965, -18.379005523261092]], [[[0, 10.050665232782423, 11.026385307998742]], [-17.82919359778298, 9.062000642947142]], [[[0, 26.526838150174818, -0.22563393232425621], [4, -33.70303936886652, 2.880339841013677]], [-17.82919359778298, 9.062000642947142]]]
test_data2 = [[[[0, 26.543274387283322, -6.262538160312672], [3, 9.937396825799755, -9.128540360867689]], [18.92765331253674, -6.460955043986683]], [[[0, 7.706544739722961, -3.758467215445748], [1, 17.03954411948937, 31.705489938553438], [3, -11.61731288777497, -6.64964096716416]], [18.92765331253674, -6.460955043986683]], [[[0, -12.35130507136378, 2.585119104239249], [1, -2.563534536165313, 38.22159657838369], [3, -26.961236804740935, -0.4802312626141525]], [-11.167066095509824, 16.592065417497455]], [[[0, 1.4138633151721272, -13.912454837810632], [1, 8.087721200818589, 20.51845934354381], [3, -17.091723454402302, -16.521500551709707], [4, -7.414211721400232, 38.09191602674439]], [-11.167066095509824, 16.592065417497455]], [[[0, 12.886743222179561, -28.703968411636318], [1, 21.660953298391387, 3.4912891084614914], [3, -6.401401414569506, -32.321583037341625], [4, 5.034079343639034, 23.102207946092893]], [-11.167066095509824, 16.592065417497455]], [[[1, 31.126317672358578, -10.036784369535214], [2, -38.70878528420893, 7.4987265861424595], [4, 17.977218575473767, 6.150889254289742]], [-6.595520680493778, -18.88118393939265]], [[[1, 41.82460922922086, 7.847527392202475], [3, 15.711709540417502, -30.34633659912818]], [-6.595520680493778, -18.88118393939265]], [[[0, 40.18454208294434, -6.710999804403755], [3, 23.019508919299156, -10.12110867290604]], [-6.595520680493778, -18.88118393939265]], [[[3, 27.18579315312821, 8.067219022708391]], [-6.595520680493778, -18.88118393939265]], [[], [11.492663265706092, 16.36822198838621]], [[[3, 24.57154567653098, 13.461499960708197]], [11.492663265706092, 16.36822198838621]], [[[0, 31.61945290413707, 0.4272295085799329], [3, 16.97392299158991, -5.274596836133088]], [11.492663265706092, 16.36822198838621]], [[[0, 22.407381798735177, -18.03500068379259], [1, 29.642444125196995, 17.3794951934614], [3, 4.7969752441371645, -21.07505361639969], [4, 14.726069092569372, 32.75999422300078]], [11.492663265706092, 16.36822198838621]], [[[0, 10.705527984670137, -34.589764174299596], [1, 18.58772336795603, -0.20109708164787765], [3, -4.839806195049413, -39.92208742305105], [4, 4.18824810165454, 14.146847823548889]], [11.492663265706092, 16.36822198838621]], [[[1, 5.878492140223764, -19.955352450942357], [4, -7.059505455306587, -0.9740849280550585]], [19.628527845173146, 3.83678180657467]], [[[1, -11.150789592446378, -22.736641053247872], [4, -28.832815721158255, -3.9462962046291388]], [-19.841703647091965, 2.5113335861604362]], [[[1, 8.64427397916182, -20.286336970889053], [4, -5.036917727942285, -6.311739993868336]], [-5.946642674882207, -19.09548221169787]], [[[0, 7.151866679283043, -39.56103232616369], [1, 16.01535401373368, -3.780995345194027], [4, -3.04801331832137, 13.697362774960865]], [-5.946642674882207, -19.09548221169787]], [[[0, 12.872879480504395, -19.707592098123207], [1, 22.236710716903136, 16.331770792606406], [3, -4.841206109583004, -21.24604435851242], [4, 4.27111163223552, 32.25309748614184]], [-5.946642674882207, -19.09548221169787]]] 
 
##  Test Case 1
##
##  Estimated Pose(s):
##      [49.999, 49.999]
##      [37.971, 33.650]
##      [26.183, 18.153]
##      [13.743, 2.114]
##      [28.095, 16.781]
##      [42.383, 30.900]
##      [55.829, 44.494]
##      [70.855, 59.697]
##      [85.695, 75.540]
##      [74.010, 92.431]
##      [53.543, 96.451]
##      [34.523, 100.078]
##      [48.621, 83.951]
##      [60.195, 68.105]
##      [73.776, 52.932]
##      [87.130, 38.536]
##      [80.301, 20.506]
##      [72.797, 2.943]
##      [55.244, 13.253]
##      [37.414, 22.315]
##  
##  Estimated Landmarks:
##      [82.954, 13.537]
##      [70.493, 74.139]
##      [36.738, 61.279]
##      [18.696, 66.057]
##      [20.633, 16.873]
 
 
##  Test Case 2
##
##  Estimated Pose(s):
##      [49.999, 49.999]
##      [69.180, 45.664]
##      [87.742, 39.702]
##      [76.269, 56.309]
##      [64.316, 72.174]
##      [52.256, 88.151]
##      [44.058, 69.399]
##      [37.001, 49.916]
##      [30.923, 30.953]
##      [23.507, 11.417]
##      [34.179, 27.131]
##      [44.154, 43.844]
##      [54.805, 60.919]
##      [65.697, 78.544]
##      [77.467, 95.624]
##      [96.801, 98.819]
##      [75.956, 99.969]
##      [70.199, 81.179]
##      [64.053, 61.721]
##      [58.106, 42.626]
##  
##  Estimated Landmarks:
##      [76.778, 42.885]
##      [85.064, 77.436]
##      [13.546, 95.649]
##      [59.448, 39.593]
##      [69.262, 94.238]
 
 
 
### Uncomment the following three lines for test case 1 ###
 
#result = slam(test_data1, 20, 5, 2.0, 2.0)
#print_result(20, 5, result)
#print result
 
 
### Uncomment the following three lines for test case 2 ###
 
#result = slam(test_data2, 20, 5, 2.0, 2.0)
#print_result(20, 5, result)
#print result

으아아아 길다. 길어 ㅠ 2부로 나누어서 포스트 하겠다.