diff --git a/.gitignore b/.gitignore
index 62e48c67..272dc63b 100644
--- a/.gitignore
+++ b/.gitignore
@@ -68,3 +68,5 @@ target/
 
 #Ipython Notebook
 .ipynb_checkpoints
+
+matplotrecorder/*
diff --git a/PathPlanning/RRTStar/rrt_star.py b/PathPlanning/RRTStar/rrt_star.py
new file mode 100644
index 00000000..509e2d01
--- /dev/null
+++ b/PathPlanning/RRTStar/rrt_star.py
@@ -0,0 +1,286 @@
+"""
+Path planning Sample Code with RRT*
+
+author: Atsushi Sakai(@Atsushi_twi)
+
+"""
+
+import copy
+import math
+import random
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+show_animation = True
+
+
+class RRTStar:
+    """
+    Class for RRT planning
+    """
+
+    class Node:
+
+        def __init__(self, x, y):
+            self.x = x
+            self.y = y
+            self.cost = 0.0
+            self.parent = None
+
+    def __init__(self, start, goal, obstacle_list, rand_area,
+                 expand_dis=0.5,
+                 goal_sample_rate=20,
+                 max_iter=500,
+                 connect_circle_dist=50.0
+                 ):
+        """
+        Setting Parameter
+
+        start:Start Position [x,y]
+        goal:Goal Position [x,y]
+        obstacleList:obstacle Positions [[x,y,size],...]
+        randArea:Random Sampling Area [min,max]
+
+        """
+        self.connect_circle_dist = connect_circle_dist
+        self.start = self.Node(start[0], start[1])
+        self.end = self.Node(goal[0], goal[1])
+        self.min_rand = rand_area[0]
+        self.max_rand = rand_area[1]
+        self.expandDis = expand_dis
+        self.goalSampleRate = goal_sample_rate
+        self.maxIter = max_iter
+        self.obstacleList = obstacle_list
+        self.node_list = []
+
+    def planning(self, animation=True, search_until_maxiter=True):
+        """
+        rrt path planning
+
+        animation: flag for animation on or off
+        search_until_maxiter: search until max iteration for path improving or not
+        """
+
+        self.node_list = [self.start]
+        for i in range(self.maxIter):
+            rnd = self.get_random_point()
+            nearest_ind = self.get_nearest_list_index(self.node_list, rnd)
+
+            new_node = self.steer(rnd, self.node_list[nearest_ind])
+
+            if self.check_collision(new_node, self.obstacleList):
+                near_inds = self.find_near_nodes(new_node)
+                new_node = self.choose_parent(new_node, near_inds)
+                if new_node:
+                    self.node_list.append(new_node)
+                self.rewire(new_node, near_inds)
+
+            if animation and i % 5 == 0:
+                self.draw_graph(rnd)
+
+            if not search_until_maxiter:  # check reaching the goal
+                d, _ = self.calc_distance_and_angle(new_node, self.end)
+                if d <= self.expandDis:
+                    return self.gen_final_course(len(self.node_list) - 1)
+
+        print("reached max iteration")
+
+        last_index = self.search_best_goal_node()
+        if last_index:
+            return self.gen_final_course(last_index)
+
+        return None
+
+    def choose_parent(self, new_node, near_inds):
+        if not near_inds:
+            return None
+
+        # search nearest cost in near_inds
+        costs = []
+        for i in near_inds:
+            d, theta = self.calc_distance_and_angle(self.node_list[i], new_node)
+            if self.check_collision_extend(self.node_list[i], theta, d):
+                costs.append(self.node_list[i].cost + d)
+            else:
+                costs.append(float("inf"))  # the cost of collision node
+        min_cost = min(costs)
+
+        if min_cost == float("inf"):
+            print("There is no good path.(min_cost is inf)")
+            return None
+
+        new_node.cost = min_cost
+        min_ind = near_inds[costs.index(min_cost)]
+        new_node.parent = self.node_list[min_ind]
+
+        return new_node
+
+    def steer(self, rnd, nearest_node):
+        new_node = self.Node(rnd[0], rnd[1])
+        d, theta = self.calc_distance_and_angle(nearest_node, new_node)
+        if d > self.expandDis:
+            new_node.x = nearest_node.x + self.expandDis * math.cos(theta)
+            new_node.y = nearest_node.y + self.expandDis * math.sin(theta)
+        new_node.cost = float("inf")
+        return new_node
+
+    def get_random_point(self):
+
+        if random.randint(0, 100) > self.goalSampleRate:
+            rnd = [random.uniform(self.min_rand, self.max_rand),
+                   random.uniform(self.min_rand, self.max_rand)]
+        else:  # goal point sampling
+            rnd = [self.end.x, self.end.y]
+
+        return rnd
+
+    def search_best_goal_node(self):
+        dist_to_goal_list = [self.calc_dist_to_goal(n.x, n.y) for n in self.node_list]
+        goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.expandDis]
+
+        if not goal_inds:
+            return None
+
+        min_cost = min([self.node_list[i].cost for i in goal_inds])
+        for i in goal_inds:
+            if self.node_list[i].cost == min_cost:
+                return i
+
+        return None
+
+    def gen_final_course(self, goal_ind):
+        path = [[self.end.x, self.end.y]]
+        node = self.node_list[goal_ind]
+        while node.parent is not None:
+            path.append([node.x, node.y])
+            node = node.parent
+        path.append([node.x, node.y])
+
+        return path
+
+    def calc_dist_to_goal(self, x, y):
+        return np.linalg.norm([x - self.end.x, y - self.end.y])
+
+    def find_near_nodes(self, new_node):
+        nnode = len(self.node_list) + 1
+        r = self.connect_circle_dist * math.sqrt((math.log(nnode) / nnode))
+        dist_list = [(node.x - new_node.x) ** 2 +
+                     (node.y - new_node.y) ** 2 for node in self.node_list]
+        near_inds = [dist_list.index(i) for i in dist_list if i <= r ** 2]
+        return near_inds
+
+    def rewire(self, new_node, near_inds):
+        for i in near_inds:
+            near_node = self.node_list[i]
+            d, theta = self.calc_distance_and_angle(near_node, new_node)
+            new_cost = new_node.cost + d
+
+            if near_node.cost > new_cost:
+                if self.check_collision_extend(near_node, theta, d):
+                    near_node.parent = new_node
+                    near_node.cost = new_cost
+                    self.propagate_cost_to_leaves(new_node)
+
+    def propagate_cost_to_leaves(self, parent_node):
+        for node in self.node_list:
+            if node.parent == parent_node:
+                d, _ = self.calc_distance_and_angle(parent_node, node)
+                node.cost = parent_node.cost + d
+                self.propagate_cost_to_leaves(node)
+
+    def check_collision_extend(self, near_node, theta, d):
+
+        tmp_node = copy.deepcopy(near_node)
+
+        for i in range(int(d / self.expandDis)):
+            tmp_node.x += self.expandDis * math.cos(theta)
+            tmp_node.y += self.expandDis * math.sin(theta)
+            if not self.check_collision(tmp_node, self.obstacleList):
+                return False
+
+        return True
+
+    def draw_graph(self, rnd=None):
+        plt.clf()
+        if rnd is not None:
+            plt.plot(rnd[0], rnd[1], "^k")
+        for node in self.node_list:
+            if node.parent is not None:
+                plt.plot([node.x, node.parent.x],
+                         [node.y, node.parent.y],
+                         "-g")
+
+        for (ox, oy, size) in self.obstacleList:
+            plt.plot(ox, oy, "ok", ms=30 * size)
+
+        plt.plot(self.start.x, self.start.y, "xr")
+        plt.plot(self.end.x, self.end.y, "xr")
+        plt.axis([-2, 15, -2, 15])
+        plt.grid(True)
+        plt.pause(0.01)
+
+    @staticmethod
+    def get_nearest_list_index(node_list, rnd):
+        dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1])
+                 ** 2 for node in node_list]
+        minind = dlist.index(min(dlist))
+
+        return minind
+
+    @staticmethod
+    def check_collision(node, obstacleList):
+        for (ox, oy, size) in obstacleList:
+            dx = ox - node.x
+            dy = oy - node.y
+            d = dx * dx + dy * dy
+            if d <= size ** 2:
+                return False  # collision
+
+        return True  # safe
+
+    @staticmethod
+    def calc_distance_and_angle(from_node, to_node):
+        dx = to_node.x - from_node.x
+        dy = to_node.y - from_node.y
+        d = math.sqrt(dx ** 2 + dy ** 2)
+        theta = math.atan2(dy, dx)
+        return d, theta
+
+
+def main():
+    print("Start " + __file__)
+
+    # ====Search Path with RRT====
+    obstacle_list = [
+        (5, 5, 1),
+        (3, 6, 2),
+        (3, 8, 2),
+        (3, 10, 2),
+        (7, 5, 2),
+        (9, 5, 2)
+    ]  # [x,y,size(radius)]
+
+    # Set Initial parameters
+    rrt = RRTStar(start=[0, 0],
+                  goal=[10, 10],
+                  rand_area=[-2, 15],
+                  obstacle_list=obstacle_list)
+    path = rrt.planning(animation=show_animation, search_until_maxiter=False)
+
+    if path is None:
+        print("Cannot find path")
+    else:
+        print("found path!!")
+
+        # Draw final path
+        if show_animation:
+            rrt.draw_graph()
+            plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
+            plt.grid(True)
+            plt.pause(0.01)  # Need for Mac
+            plt.show()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/PathPlanning/RRTstar/rrt_star.py b/PathPlanning/RRTstar/rrt_star.py
deleted file mode 100644
index 169ead82..00000000
--- a/PathPlanning/RRTstar/rrt_star.py
+++ /dev/null
@@ -1,285 +0,0 @@
-
-
-
-"""
-Path Planning Sample Code with RRT*
-
-author: AtsushiSakai(@Atsushi_twi)
-
-"""
-
-import random
-import math
-import copy
-import numpy as np
-import matplotlib.pyplot as plt
-
-show_animation = True
-
-
-class Node():
-
-    def __init__(self, x, y):
-        self.x = x
-        self.y = y
-        self.cost = 0.0
-        self.parent = None
-
-
-class RRT():
-    """
-    Class for RRT Planning
-    """
-
-    def __init__(self, start, goal, obstacleList, randArea,
-                 expandDis=0.5, goalSampleRate=20, maxIter=500):
-        """
-        Setting Parameter
-
-        start:Start Position [x,y]
-        goal:Goal Position [x,y]
-        obstacleList:obstacle Positions [[x,y,size],...]
-        randArea:Ramdom Samping Area [min,max]
-
-        """
-        self.start = Node(start[0], start[1])
-        self.end = Node(goal[0], goal[1])
-        self.minrand = randArea[0]
-        self.maxrand = randArea[1]
-        self.expandDis = expandDis
-        self.goalSampleRate = goalSampleRate
-        self.maxIter = maxIter
-        self.obstacleList = obstacleList
-
-    def Planning(self, animation=True, search_until_maxiter=True):
-        """
-        rrt path planning
-
-        animation: flag for animation on or off
-        search_until_maxiter: search until max iteration for path improving or not
-        """
-
-        self.nodeList = [self.start]
-        for i in range(self.maxIter):
-            rnd = self.get_random_point()
-            nind = self.GetNearestListIndex(self.nodeList, rnd)
-
-            new_node = self.steer(rnd, nind)
-
-            if self.__CollisionCheck(new_node, self.obstacleList):
-                nearinds = self.find_near_nodes(new_node)
-                new_node = self.choose_parent(new_node, nearinds)
-                self.nodeList.append(new_node)
-                self.rewire(new_node, nearinds)
-
-            if animation and i % 5 == 0:
-                self.DrawGraph(rnd)
-
-            # generate course
-            if not search_until_maxiter:
-                lastIndex = self.get_best_last_index()
-                if lastIndex:
-                    return self.gen_final_course(lastIndex)
-
-        print("reached max iteration")
-
-        lastIndex = self.get_best_last_index()
-        if lastIndex:
-            return self.gen_final_course(lastIndex)
-
-        return None
-
-    def choose_parent(self, new_node, nearinds):
-        if not nearinds:
-            return new_node
-
-        dlist = []
-        for i in nearinds:
-            dx = new_node.x - self.nodeList[i].x
-            dy = new_node.y - self.nodeList[i].y
-            d = math.sqrt(dx ** 2 + dy ** 2)
-            theta = math.atan2(dy, dx)
-            if self.check_collision_extend(self.nodeList[i], theta, d):
-                dlist.append(self.nodeList[i].cost + d)
-            else:
-                dlist.append(float("inf"))
-
-        mincost = min(dlist)
-        minind = nearinds[dlist.index(mincost)]
-
-        if mincost == float("inf"):
-            print("mincost is inf")
-            return new_node
-
-        new_node.cost = mincost
-        new_node.parent = minind
-
-        return new_node
-
-    def steer(self, rnd, nind):
-
-        # expand tree
-        nearest_node = self.nodeList[nind]
-        theta = math.atan2(rnd[1] - nearest_node.y, rnd[0] - nearest_node.x)
-        new_node = Node(rnd[0], rnd[1])
-        currentDistance = math.sqrt(
-            (rnd[1] - nearest_node.y) ** 2 + (rnd[0] - nearest_node.x) ** 2)
-        # Find a point within expandDis of nind, and closest to rnd
-        if currentDistance <= self.expandDis:
-            pass
-        else:
-            new_node.x = nearest_node.x + self.expandDis * math.cos(theta)
-            new_node.y = nearest_node.y + self.expandDis * math.sin(theta)
-        new_node.cost = float("inf")
-        new_node.parent = None
-        return new_node
-
-    def get_random_point(self):
-
-        if random.randint(0, 100) > self.goalSampleRate:
-            rnd = [random.uniform(self.minrand, self.maxrand),
-                   random.uniform(self.minrand, self.maxrand)]
-        else:  # goal point sampling
-            rnd = [self.end.x, self.end.y]
-
-        return rnd
-
-    def get_best_last_index(self):
-
-        disglist = [self.calc_dist_to_goal(
-            node.x, node.y) for node in self.nodeList]
-        goalinds = [disglist.index(i) for i in disglist if i <= self.expandDis]
-
-        if not goalinds:
-            return None
-
-        mincost = min([self.nodeList[i].cost for i in goalinds])
-        for i in goalinds:
-            if self.nodeList[i].cost == mincost:
-                return i
-
-        return None
-
-    def gen_final_course(self, goalind):
-        path = [[self.end.x, self.end.y]]
-        while self.nodeList[goalind].parent is not None:
-            node = self.nodeList[goalind]
-            path.append([node.x, node.y])
-            goalind = node.parent
-        path.append([self.start.x, self.start.y])
-        return path
-
-    def calc_dist_to_goal(self, x, y):
-        return np.linalg.norm([x - self.end.x, y - self.end.y])
-
-    def find_near_nodes(self, new_node):
-        nnode = len(self.nodeList)
-        r = 50.0 * math.sqrt((math.log(nnode) / nnode))
-        dlist = [(node.x - new_node.x) ** 2 +
-                 (node.y - new_node.y) ** 2 for node in self.nodeList]
-        nearinds = [dlist.index(i) for i in dlist if i <= r ** 2]
-        return nearinds
-
-    def rewire(self, new_node, nearinds):
-        nnode = len(self.nodeList)
-        for i in nearinds:
-            nearNode = self.nodeList[i]
-
-            dx = new_node.x - nearNode.x
-            dy = new_node.y - nearNode.y
-            d = math.sqrt(dx ** 2 + dy ** 2)
-
-            scost = new_node.cost + d
-
-            if nearNode.cost > scost:
-                theta = math.atan2(dy, dx)
-                if self.check_collision_extend(nearNode, theta, d):
-                    nearNode.parent = nnode - 1
-                    nearNode.cost = scost
-
-    def check_collision_extend(self, nearNode, theta, d):
-
-        tmpNode = copy.deepcopy(nearNode)
-
-        for i in range(int(d / self.expandDis)):
-            tmpNode.x += self.expandDis * math.cos(theta)
-            tmpNode.y += self.expandDis * math.sin(theta)
-            if not self.__CollisionCheck(tmpNode, self.obstacleList):
-                return False
-
-        return True
-
-    def DrawGraph(self, rnd=None):
-        """
-        Draw Graph
-        """
-        plt.clf()
-        if rnd is not None:
-            plt.plot(rnd[0], rnd[1], "^k")
-        for node in self.nodeList:
-            if node.parent is not None:
-                plt.plot([node.x, self.nodeList[node.parent].x], [
-                         node.y, self.nodeList[node.parent].y], "-g")
-
-        for (ox, oy, size) in self.obstacleList:
-            plt.plot(ox, oy, "ok", ms=30 * size)
-
-        plt.plot(self.start.x, self.start.y, "xr")
-        plt.plot(self.end.x, self.end.y, "xr")
-        plt.axis([-2, 15, -2, 15])
-        plt.grid(True)
-        plt.pause(0.01)
-
-    def GetNearestListIndex(self, nodeList, rnd):
-        dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1])
-                 ** 2 for node in nodeList]
-        minind = dlist.index(min(dlist))
-
-        return minind
-
-    def __CollisionCheck(self, node, obstacleList):
-        for (ox, oy, size) in obstacleList:
-            dx = ox - node.x
-            dy = oy - node.y
-            d = dx * dx + dy * dy
-            if d <= size ** 2:
-                return False  # collision
-
-        return True  # safe
-
-
-def main():
-    print("Start " + __file__)
-
-    # ====Search Path with RRT====
-    obstacleList = [
-        (5, 5, 1),
-        (3, 6, 2),
-        (3, 8, 2),
-        (3, 10, 2),
-        (7, 5, 2),
-        (9, 5, 2)
-    ]  # [x,y,size(radius)]
-
-    # Set Initial parameters
-    rrt = RRT(start=[0, 0], goal=[10, 10],
-              randArea=[-2, 15], obstacleList=obstacleList)
-    path = rrt.Planning(animation=show_animation, search_until_maxiter=False)
-
-    if path is None:
-        print("Cannot find path")
-    else:
-        print("found path!!")
-
-        # Draw final path
-        if show_animation:
-            rrt.DrawGraph()
-            plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
-            plt.grid(True)
-            plt.pause(0.01)  # Need for Mac
-            plt.show()
-
-
-if __name__ == '__main__':
-    main()
-
diff --git a/PathTracking/.gitignore b/PathTracking/.gitignore
deleted file mode 100644
index e69de29b..00000000
diff --git a/tests/test_rrt_star.py b/tests/test_rrt_star.py
index 2171402c..c5f8f870 100644
--- a/tests/test_rrt_star.py
+++ b/tests/test_rrt_star.py
@@ -3,7 +3,7 @@ import sys
 from unittest import TestCase
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
-                "/../PathPlanning/RRTstar/")
+                "/../PathPlanning/RRTStar/")
 
 try:
     import rrt_star as m