diff --git a/.github/FUNDING.yml b/.github/FUNDING.yml
index 80f4984c..24f09262 100644
--- a/.github/FUNDING.yml
+++ b/.github/FUNDING.yml
@@ -1,3 +1,4 @@
 # These are supported funding model platforms
+github:  AtsushiSakai
 patreon: myenigma
 custom: https://www.paypal.me/myenigmapay/
diff --git a/.github/workflows/greetings.yml b/.github/workflows/greetings.yml
new file mode 100644
index 00000000..3da4a694
--- /dev/null
+++ b/.github/workflows/greetings.yml
@@ -0,0 +1,13 @@
+name: Greetings
+
+on: [pull_request, issues]
+
+jobs:
+  greeting:
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/first-interaction@v1
+      with:
+        repo-token: ${{ secrets.GITHUB_TOKEN }}
+        issue-message: 'Message that will be displayed on users'' This is first issue for you on this project'
+        pr-message: 'Message that will be displayed on users'' This is first pr for you on this project'
diff --git a/.github/workflows/pythonpackage.yml b/.github/workflows/pythonpackage.yml
new file mode 100644
index 00000000..a3203ff1
--- /dev/null
+++ b/.github/workflows/pythonpackage.yml
@@ -0,0 +1,34 @@
+name: Python package
+
+on: [push]
+
+jobs:
+  build:
+
+    runs-on: ubuntu-latest
+    strategy:
+      max-parallel: 4
+      matrix:
+        python-version: [3.6, 3.7]
+
+    steps:
+    - uses: actions/checkout@v1
+    - name: Set up Python ${{ matrix.python-version }}
+      uses: actions/setup-python@v1
+      with:
+        python-version: ${{ matrix.python-version }}
+    - name: Install dependencies
+      run: |
+        python -m pip install --upgrade pip
+        pip install -r requirements.txt
+    - name: Lint with flake8
+      run: |
+        pip install flake8
+        # stop the build if there are Python syntax errors or undefined names
+        flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics
+        # exit-zero treats all errors as warnings. The GitHub editor is 127 chars wide
+        flake8 . --count --exit-zero --max-complexity=10 --max-line-length=127 --statistics
+    - name: install coverage
+      run: pip install coverage   
+    - name: Test
+      run: bash runtests.sh
diff --git a/ArmNavigation/n_joint_arm_3d/NLinkArm.py b/ArmNavigation/n_joint_arm_3d/NLinkArm.py
new file mode 100644
index 00000000..da562d9f
--- /dev/null
+++ b/ArmNavigation/n_joint_arm_3d/NLinkArm.py
@@ -0,0 +1,196 @@
+"""
+Class of n-link arm in 3D
+Author: Takayuki Murooka (takayuki5168)
+"""
+import numpy as np
+import math
+from mpl_toolkits.mplot3d import Axes3D
+import matplotlib.pyplot as plt
+
+class Link:
+    def __init__(self, dh_params):
+        self.dh_params_ = dh_params
+
+    def transformation_matrix(self):
+        theta = self.dh_params_[0]
+        alpha = self.dh_params_[1]
+        a = self.dh_params_[2]
+        d = self.dh_params_[3]
+
+        st = math.sin(theta)
+        ct = math.cos(theta)
+        sa = math.sin(alpha)
+        ca = math.cos(alpha)
+        trans = np.array([[ct, -st * ca, st * sa, a * ct],
+                          [st, ct * ca, -ct * sa, a * st],
+                          [0, sa, ca, d],
+                          [0, 0, 0, 1]])
+
+        return trans
+
+    def basic_jacobian(self, trans_prev, ee_pos):
+        pos_prev = np.array([trans_prev[0, 3], trans_prev[1, 3], trans_prev[2, 3]])
+        z_axis_prev = np.array([trans_prev[0, 2], trans_prev[1, 2], trans_prev[2, 2]])
+
+        basic_jacobian = np.hstack((np.cross(z_axis_prev, ee_pos - pos_prev), z_axis_prev))
+        return basic_jacobian
+        
+
+class NLinkArm:
+    def __init__(self, dh_params_list):
+        self.link_list = []
+        for i in range(len(dh_params_list)):
+            self.link_list.append(Link(dh_params_list[i]))
+
+    def transformation_matrix(self):
+        trans = np.identity(4)
+        for i in range(len(self.link_list)):
+            trans = np.dot(trans, self.link_list[i].transformation_matrix())
+        return trans
+    
+    def forward_kinematics(self, plot=False):
+        trans = self.transformation_matrix()
+
+        x = trans[0, 3]
+        y = trans[1, 3]
+        z = trans[2, 3]
+        alpha, beta, gamma = self.euler_angle()
+
+        if plot:
+            self.fig = plt.figure()
+            self.ax = Axes3D(self.fig)
+            
+            x_list = []
+            y_list = []
+            z_list = []
+    
+            trans = np.identity(4)
+            
+            x_list.append(trans[0, 3])
+            y_list.append(trans[1, 3])
+            z_list.append(trans[2, 3])
+            for i in range(len(self.link_list)):
+                trans = np.dot(trans, self.link_list[i].transformation_matrix())
+                x_list.append(trans[0, 3])
+                y_list.append(trans[1, 3])
+                z_list.append(trans[2, 3])
+                
+            self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5)
+            self.ax.plot([0], [0], [0], "o")
+            
+            self.ax.set_xlim(-1, 1)
+            self.ax.set_ylim(-1, 1)
+            self.ax.set_zlim(-1, 1)
+  
+            plt.show()
+
+        return [x, y, z, alpha, beta, gamma]
+
+    def basic_jacobian(self):
+        ee_pos = self.forward_kinematics()[0:3]
+        basic_jacobian_mat = []
+        
+        trans = np.identity(4)        
+        for i in range(len(self.link_list)):
+            basic_jacobian_mat.append(self.link_list[i].basic_jacobian(trans, ee_pos))
+            trans = np.dot(trans, self.link_list[i].transformation_matrix())
+            
+        return np.array(basic_jacobian_mat).T
+
+    def inverse_kinematics(self, ref_ee_pose, plot=False):
+        for cnt in range(500):
+            ee_pose = self.forward_kinematics()
+            diff_pose = np.array(ref_ee_pose) - ee_pose
+        
+            basic_jacobian_mat = self.basic_jacobian()
+            alpha, beta, gamma = self.euler_angle()
+            
+            K_zyz = np.array([[0, -math.sin(alpha), math.cos(alpha) * math.sin(beta)],
+                              [0, math.cos(alpha), math.sin(alpha) * math.sin(beta)],
+                              [1, 0, math.cos(beta)]])
+            K_alpha = np.identity(6)
+            K_alpha[3:, 3:] = K_zyz
+
+            theta_dot = np.dot(np.dot(np.linalg.pinv(basic_jacobian_mat), K_alpha), np.array(diff_pose))
+            self.update_joint_angles(theta_dot / 100.)
+            
+        if plot:
+            self.fig = plt.figure()
+            self.ax = Axes3D(self.fig)
+            
+            x_list = []
+            y_list = []
+            z_list = []
+    
+            trans = np.identity(4)
+            
+            x_list.append(trans[0, 3])
+            y_list.append(trans[1, 3])
+            z_list.append(trans[2, 3])
+            for i in range(len(self.link_list)):
+                trans = np.dot(trans, self.link_list[i].transformation_matrix())
+                x_list.append(trans[0, 3])
+                y_list.append(trans[1, 3])
+                z_list.append(trans[2, 3])
+                
+            self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5)
+            self.ax.plot([0], [0], [0], "o")
+            
+            self.ax.set_xlim(-1, 1)
+            self.ax.set_ylim(-1, 1)
+            self.ax.set_zlim(-1, 1)
+  
+            self.ax.plot([ref_ee_pose[0]], [ref_ee_pose[1]], [ref_ee_pose[2]], "o")
+            plt.show()
+                
+    def euler_angle(self):
+        trans = self.transformation_matrix()
+        
+        alpha = math.atan2(trans[1][2], trans[0][2])
+        if not (-math.pi / 2 <= alpha and alpha <= math.pi / 2):
+            alpha = math.atan2(trans[1][2], trans[0][2]) + math.pi
+        if not (-math.pi / 2 <= alpha and alpha <= math.pi / 2):
+            alpha = math.atan2(trans[1][2], trans[0][2]) - math.pi
+        beta = math.atan2(trans[0][2] * math.cos(alpha) + trans[1][2] * math.sin(alpha), trans[2][2])
+        gamma = math.atan2(-trans[0][0] * math.sin(alpha) + trans[1][0] * math.cos(alpha), -trans[0][1] * math.sin(alpha) + trans[1][1] * math.cos(alpha))
+        
+        return alpha, beta, gamma
+    
+    def set_joint_angles(self, joint_angle_list):
+        for i in range(len(self.link_list)):
+            self.link_list[i].dh_params_[0] = joint_angle_list[i]
+
+    def update_joint_angles(self, diff_joint_angle_list):
+        for i in range(len(self.link_list)):
+            self.link_list[i].dh_params_[0] += diff_joint_angle_list[i]
+        
+    def plot(self):
+        self.fig = plt.figure()
+        self.ax = Axes3D(self.fig)
+        
+        x_list = []
+        y_list = []
+        z_list = []
+
+        trans = np.identity(4)
+        
+        x_list.append(trans[0, 3])
+        y_list.append(trans[1, 3])
+        z_list.append(trans[2, 3])
+        for i in range(len(self.link_list)):
+            trans = np.dot(trans, self.link_list[i].transformation_matrix())
+            x_list.append(trans[0, 3])
+            y_list.append(trans[1, 3])
+            z_list.append(trans[2, 3])
+            
+        self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5)
+        self.ax.plot([0], [0], [0], "o")
+
+        self.ax.set_xlabel("x")
+        self.ax.set_ylabel("y")
+        self.ax.set_zlabel("z")        
+        
+        self.ax.set_xlim(-1, 1)
+        self.ax.set_ylim(-1, 1)
+        self.ax.set_zlim(-1, 1)        
+        plt.show()
diff --git a/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py
new file mode 100644
index 00000000..87907f80
--- /dev/null
+++ b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py
@@ -0,0 +1,30 @@
+"""
+Forward Kinematics for an n-link arm in 3D
+Author: Takayuki Murooka (takayuki5168)
+"""
+import math
+from NLinkArm import NLinkArm
+import random
+
+def random_val(min_val, max_val):
+    return min_val + random.random() * (max_val - min_val)
+
+
+if __name__ == "__main__":
+    print("Start solving Forward Kinematics 10 times")
+
+    # init NLinkArm with Denavit-Hartenberg parameters of PR2    
+    n_link_arm = NLinkArm([[0., -math.pi/2, .1, 0.],
+                           [math.pi/2, math.pi/2, 0., 0.],
+                           [0., -math.pi/2, 0., .4],
+                           [0., math.pi/2, 0., 0.],
+                           [0., -math.pi/2, 0., .321],
+                           [0., math.pi/2, 0., 0.],
+                           [0., 0., 0., 0.]])
+
+    # execute FK 10 times
+    for i in range(10):
+        n_link_arm.set_joint_angles([random_val(-1, 1) for j in range(len(n_link_arm.link_list))])
+        
+        ee_pose = n_link_arm.forward_kinematics(plot=True)
+
diff --git a/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py
new file mode 100644
index 00000000..44be2270
--- /dev/null
+++ b/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py
@@ -0,0 +1,32 @@
+"""
+Inverse Kinematics for an n-link arm in 3D
+Author: Takayuki Murooka (takayuki5168)
+"""
+import math
+from NLinkArm import NLinkArm
+import random
+
+
+def random_val(min_val, max_val):
+    return min_val + random.random() * (max_val - min_val)
+
+if __name__ == "__main__":
+    print("Start solving Inverse Kinematics 10 times")
+
+    # init NLinkArm with Denavit-Hartenberg parameters of PR2        
+    n_link_arm = NLinkArm([[0., -math.pi/2, .1, 0.],
+                           [math.pi/2, math.pi/2, 0., 0.],
+                           [0., -math.pi/2, 0., .4],
+                           [0., math.pi/2, 0., 0.],
+                           [0., -math.pi/2, 0., .321],
+                           [0., math.pi/2, 0., 0.],
+                           [0., 0., 0., 0.]])
+
+    # execute IK 10 times
+    for i in range(10):
+        n_link_arm.inverse_kinematics([random_val(-0.5, 0.5),
+                                       random_val(-0.5, 0.5),
+                                       random_val(-0.5, 0.5),
+                                       random_val(-0.5, 0.5),
+                                       random_val(-0.5, 0.5),
+                                       random_val(-0.5, 0.5)], plot=True)
diff --git a/Localization/Kalmanfilter_basics.ipynb b/Localization/Kalmanfilter_basics.ipynb
index 37428791..b4913f9e 100644
--- a/Localization/Kalmanfilter_basics.ipynb
+++ b/Localization/Kalmanfilter_basics.ipynb
@@ -228,7 +228,7 @@
     "\n",
     "#### Central Limit Theorem\n",
     "\n",
-    "According to this theorem, the average of n samples of random and independant variables tends to follow a normal distribution as we increase the sample size.(Generally, for n>=30)"
+    "According to this theorem, the average of n samples of random and independent variables tends to follow a normal distribution as we increase the sample size.(Generally, for n>=30)"
    ]
   },
   {
diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
index ebf254e1..c8a44349 100644
--- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
+++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
@@ -2,7 +2,7 @@
 
 Mobile robot motion planning sample with Dynamic Window Approach
 
-author: Atsushi Sakai (@Atsushi_twi), Goktug Karakasli
+author: Atsushi Sakai (@Atsushi_twi), Göktuğ Karakaşlı
 
 """
 
diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py
index 4a32801d..17574918 100644
--- a/PathPlanning/InformedRRTStar/informed_rrt_star.py
+++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py
@@ -40,7 +40,6 @@ class InformedRRTStar:
         self.node_list = [self.start]
         # max length we expect to find in our 'informed' sample space, starts as infinite
         cBest = float('inf')
-        pathLen = float('inf')
         solutionSet = set()
         path = None
 
@@ -74,10 +73,9 @@ class InformedRRTStar:
             newNode = self.get_new_node(theta, nind, nearestNode)
             d = self.line_cost(nearestNode, newNode)
 
-            isCollision = self.collision_check(newNode, self.obstacle_list)
-            isCollisionEx = self.check_collision_extend(nearestNode, theta, d)
+            noCollision = self.check_collision(nearestNode, theta, d)
 
-            if isCollision and isCollisionEx:
+            if noCollision:
                 nearInds = self.find_near_nodes(newNode)
                 newNode = self.choose_parent(newNode, nearInds)
 
@@ -85,14 +83,14 @@ class InformedRRTStar:
                 self.rewire(newNode, nearInds)
 
                 if self.is_near_goal(newNode):
-                    solutionSet.add(newNode)
-                    lastIndex = len(self.node_list) - 1
-                    tempPath = self.get_final_course(lastIndex)
-                    tempPathLen = self.get_path_len(tempPath)
-                    if tempPathLen < pathLen:
-                        path = tempPath
-                        cBest = tempPathLen
-
+                    if self.check_segment_collision(newNode.x, newNode.y, self.goal.x , self.goal.y):
+                        solutionSet.add(newNode)
+                        lastIndex = len(self.node_list) - 1
+                        tempPath = self.get_final_course(lastIndex)
+                        tempPathLen = self.get_path_len(tempPath)
+                        if tempPathLen < cBest:
+                            path = tempPath
+                            cBest = tempPathLen
             if animation:
                 self.draw_graph(xCenter=xCenter,
                                 cBest=cBest, cMin=cMin,
@@ -110,7 +108,7 @@ class InformedRRTStar:
             dy = newNode.y - self.node_list[i].y
             d = math.sqrt(dx ** 2 + dy ** 2)
             theta = math.atan2(dy, dx)
-            if self.check_collision_extend(self.node_list[i], theta, d):
+            if self.check_collision(self.node_list[i], theta, d):
                 dList.append(self.node_list[i].cost + d)
             else:
                 dList.append(float('inf'))
@@ -194,17 +192,6 @@ class InformedRRTStar:
         minIndex = dList.index(min(dList))
         return minIndex
 
-    @staticmethod
-    def collision_check(newNode, obstacleList):
-        for (ox, oy, size) in obstacleList:
-            dx = ox - newNode.x
-            dy = oy - newNode.y
-            d = dx * dx + dy * dy
-            if d <= 1.1 * size ** 2:
-                return False  # collision
-
-        return True  # safe
-
     def get_new_node(self, theta, nind, nearestNode):
         newNode = copy.deepcopy(nearestNode)
 
@@ -234,21 +221,41 @@ class InformedRRTStar:
             if nearNode.cost > scost:
                 theta = math.atan2(newNode.y - nearNode.y,
                                    newNode.x - nearNode.x)
-                if self.check_collision_extend(nearNode, theta, d):
+                if self.check_collision(nearNode, theta, d):
                     nearNode.parent = n_node - 1
                     nearNode.cost = scost
+                    
+    @staticmethod
+    def distance_squared_point_to_segment(v, w, p):
+        # Return minimum distance between line segment vw and point p
+        if (np.array_equal(v, w)):
+            return (p-v).dot(p-v) # v == w case
+        l2 = (w-v).dot(w-v) # i.e. |w-v|^2 -  avoid a sqrt
+        # Consider the line extending the segment, parameterized as v + t (w - v).
+        # We find projection of point p onto the line. 
+        # It falls where t = [(p-v) . (w-v)] / |w-v|^2
+        # We clamp t from [0,1] to handle points outside the segment vw.
+        t = max(0, min(1, (p - v).dot(w - v) / l2))
+        projection = v + t * (w - v) # Projection falls on the segment
+        return (p-projection).dot(p-projection)
 
-    def check_collision_extend(self, nearNode, theta, d):
-        tmpNode = copy.deepcopy(nearNode)
-
-        for i in range(int(d / self.expand_dis)):
-            tmpNode.x += self.expand_dis * math.cos(theta)
-            tmpNode.y += self.expand_dis * math.sin(theta)
-            if not self.collision_check(tmpNode, self.obstacle_list):
-                return False
-
+    def check_segment_collision(self, x1, y1, x2, y2):
+        for (ox, oy, size) in self.obstacle_list:
+            dd = self.distance_squared_point_to_segment(
+                np.array([x1, y1]),
+                np.array([x2, y2]),
+                np.array([ox, oy]))
+            if dd <= size**2:
+                return False  # collision
         return True
 
+
+    def check_collision(self, nearNode, theta, d):
+        tmpNode = copy.deepcopy(nearNode)
+        endx = tmpNode.x + math.cos(theta)*d
+        endy = tmpNode.y + math.sin(theta)*d
+        return self.check_segment_collision(tmpNode.x, tmpNode.y, endx, endy)
+
     def get_final_course(self, lastIndex):
         path = [[self.goal.x, self.goal.y]]
         while self.node_list[lastIndex].parent is not None:
diff --git a/README.md b/README.md
index 1c455e92..e740049b 100644
--- a/README.md
+++ b/README.md
@@ -31,8 +31,6 @@ Python codes for robotics algorithm.
    * [SLAM](#slam)
       * [Iterative Closest Point (ICP) Matching](#iterative-closest-point-icp-matching)
       * [FastSLAM 1.0](#fastslam-10)
-      * [Pose Optimization SLAM 2D](#pose-optimization-slam-2d)
-      * [Pose Optimization SLAM 3D](#pose-optimization-slam-3d)
    * [Path Planning](#path-planning)
       * [Dynamic Window Approach](#dynamic-window-approach)
       * [Grid based search](#grid-based-search)
@@ -251,21 +249,6 @@ Ref:
 - [SLAM simulations by Tim Bailey](http://www-personal.acfr.usyd.edu.au/tbailey/software/slam_simulations.htm)
 
 
-## Pose Optimization SLAM 2D
-
-This is a graph based 2D pose optimization SLAM example.
-
-![3](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/PoseOptimizationSLAM2D/animation.gif)
-
-
-## Pose Optimization SLAM 3D
-
-This is a graph based 3D pose optimization SLAM example.
-
-![3](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/PoseOptimizationSLAM3D/animation.gif)
-
-
-
 # Path Planning
 
 ## Dynamic Window Approach
diff --git a/SLAM/EKFSLAM/ekf_slam.ipynb b/SLAM/EKFSLAM/ekf_slam.ipynb
index 6509373d..f8009395 100644
--- a/SLAM/EKFSLAM/ekf_slam.ipynb
+++ b/SLAM/EKFSLAM/ekf_slam.ipynb
@@ -479,7 +479,7 @@
     "    \"\"\"\n",
     "    sq = math.sqrt(q)\n",
     "    G = np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],\n",
-    "                  [delta[1, 0], - delta[0, 0], - 1.0, - delta[1, 0], delta[0, 0]]])\n",
+    "                  [delta[1, 0], - delta[0, 0], - q, - delta[1, 0], delta[0, 0]]])\n",
     "\n",
     "    G = G / q\n",
     "    nLM = calc_n_LM(x)\n",
diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py
index ae39c9e0..27e50354 100644
--- a/SLAM/EKFSLAM/ekf_slam.py
+++ b/SLAM/EKFSLAM/ekf_slam.py
@@ -1,5 +1,6 @@
 """
 Extended Kalman Filter SLAM example
+
 author: Atsushi Sakai (@Atsushi_twi)
 """
 
@@ -35,10 +36,10 @@ def ekf_slam(xEst, PEst, u, z):
 
     # Update
     for iz in range(len(z[:, 0])):  # for each observation
-        minid = search_correspond_landmark_id(xEst, PEst, z[iz, 0:2])
+        min_id = search_correspond_landmark_id(xEst, PEst, z[iz, 0:2])
 
         nLM = calc_n_lm(xEst)
-        if minid == nLM:
+        if min_id == nLM:
             print("New LM")
             # Extend state and covariance matrix
             xAug = np.vstack((xEst, calc_landmark_position(xEst, z[iz, :])))
@@ -46,8 +47,8 @@ def ekf_slam(xEst, PEst, u, z):
                               np.hstack((np.zeros((LM_SIZE, len(xEst))), initP))))
             xEst = xAug
             PEst = PAug
-        lm = get_landmark_position_from_state(xEst, minid)
-        y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], minid)
+        lm = get_landmark_position_from_state(xEst, min_id)
+        y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], min_id)
 
         K = (PEst @ H.T) @ np.linalg.inv(S)
         xEst = xEst + (K @ y)
@@ -60,8 +61,8 @@ def ekf_slam(xEst, PEst, u, z):
 
 def calc_input():
     v = 1.0  # [m/s]
-    yawrate = 0.1  # [rad/s]
-    u = np.array([[v, yawrate]]).T
+    yaw_rate = 0.1  # [rad/s]
+    u = np.array([[v, yaw_rate]]).T
     return u
 
 
@@ -79,8 +80,8 @@ def observation(xTrue, xd, u, RFID):
         angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
         if d <= MAX_RANGE:
             dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5  # add noise
-            anglen = angle + np.random.randn() * Q_sim[1, 1] ** 0.5  # add noise
-            zi = np.array([dn, anglen, i])
+            angle_n = angle + np.random.randn() * Q_sim[1, 1] ** 0.5  # add noise
+            zi = np.array([dn, angle_n, i])
             z = np.vstack((z, zi))
 
     # add noise to input
@@ -128,8 +129,6 @@ def calc_landmark_position(x, z):
 
     zp[0, 0] = x[0, 0] + z[0] * math.cos(x[2, 0] + z[1])
     zp[1, 0] = x[1, 0] + z[0] * math.sin(x[2, 0] + z[1])
-    # zp[0, 0] = x[0, 0] + z[0, 0] * math.cos(x[2, 0] + z[0, 1])
-    # zp[1, 0] = x[1, 0] + z[0, 0] * math.sin(x[2, 0] + z[0, 1])
 
     return zp
 
@@ -147,25 +146,25 @@ def search_correspond_landmark_id(xAug, PAug, zi):
 
     nLM = calc_n_lm(xAug)
 
-    mdist = []
+    min_dist = []
 
     for i in range(nLM):
         lm = get_landmark_position_from_state(xAug, i)
         y, S, H = calc_innovation(lm, xAug, PAug, zi, i)
-        mdist.append(y.T @ np.linalg.inv(S) @ y)
+        min_dist.append(y.T @ np.linalg.inv(S) @ y)
 
-    mdist.append(M_DIST_TH)  # new landmark
+    min_dist.append(M_DIST_TH)  # new landmark
 
-    minid = mdist.index(min(mdist))
+    min_id = min_dist.index(min(min_dist))
 
-    return minid
+    return min_id
 
 
 def calc_innovation(lm, xEst, PEst, z, LMid):
     delta = lm - xEst[0:2]
     q = (delta.T @ delta)[0, 0]
-    zangle = math.atan2(delta[1, 0], delta[0, 0]) - xEst[2, 0]
-    zp = np.array([[math.sqrt(q), pi_2_pi(zangle)]])
+    z_angle = math.atan2(delta[1, 0], delta[0, 0]) - xEst[2, 0]
+    zp = np.array([[math.sqrt(q), pi_2_pi(z_angle)]])
     y = (z - zp).T
     y[1] = pi_2_pi(y[1])
     H = jacob_h(q, delta, xEst, LMid + 1)
@@ -177,7 +176,7 @@ def calc_innovation(lm, xEst, PEst, z, LMid):
 def jacob_h(q, delta, x, i):
     sq = math.sqrt(q)
     G = np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],
-                  [delta[1, 0], - delta[0, 0], - 1.0, - delta[1, 0], delta[0, 0]]])
+                  [delta[1, 0], - delta[0, 0], - q, - delta[1, 0], delta[0, 0]]])
 
     G = G / q
     nLM = calc_n_lm(x)
diff --git a/SLAM/PoseOptimizationSLAM2D/README.md b/SLAM/PoseOptimizationSLAM2D/README.md
deleted file mode 100644
index 612fa518..00000000
--- a/SLAM/PoseOptimizationSLAM2D/README.md
+++ /dev/null
@@ -1,9 +0,0 @@
-# How to use
-
-1. Download data
-
->$ python data_downloader.py
-
-2. run SLAM
-
->$ python pose_optimization_slam.py
diff --git a/SLAM/PoseOptimizationSLAM2D/data_downloader.py b/SLAM/PoseOptimizationSLAM2D/data_downloader.py
deleted file mode 100644
index d1b8a2c2..00000000
--- a/SLAM/PoseOptimizationSLAM2D/data_downloader.py
+++ /dev/null
@@ -1,28 +0,0 @@
-"""
-
-Data down loader for pose optimization
-
-author: Atsushi Sakai
-
-"""
-
-
-import subprocess
-def main():
-    print("start!!")
-
-    cmd = "wget https://www.dropbox.com/s/vcz8cag7bo0zlaj/input_INTEL_g2o.g2o?dl=0 -O intel.g2o -nc"
-    subprocess.call(cmd, shell=True)
-
-    cmd = "wget https://www.dropbox.com/s/d8fcn1jg1mebx8f/input_MITb_g2o.g2o?dl=0 -O mit_killian.g2o -nc"
-
-    subprocess.call(cmd, shell=True)
-    cmd = "wget https://www.dropbox.com/s/gmdzo74b3tzvbrw/input_M3500_g2o.g2o?dl=0 -O manhattan3500.g2o -nc"
-    subprocess.call(cmd, shell=True)
-
-    print("done!!")
-
-
-if __name__ == '__main__':
-    main()
-
diff --git a/SLAM/PoseOptimizationSLAM3D/README.md b/SLAM/PoseOptimizationSLAM3D/README.md
deleted file mode 100644
index 396a6297..00000000
--- a/SLAM/PoseOptimizationSLAM3D/README.md
+++ /dev/null
@@ -1,25 +0,0 @@
-# PoseOptimizationSLAM3D
-3D (x, y, z, qw, qx, qy, qz) pose optimization SLAM
-
-## How to use 
-1. Download data  
-
-~~~
-python data_downloader.py
-~~~
-
-2. run SLAM 
-
-~~~
-python pose_optimization_slam_3d.py
-~~~
-
-## Reference 
-- [A Compact and Portable Implementation of Graph\-based SLAM](https://www.researchgate.net/publication/321287640_A_Compact_and_Portable_Implementation_of_Graph-based_SLAM)  
-
-- [GitHub \- furo\-org/p2o: Single header 2D/3D graph\-based SLAM library](https://github.com/furo-org/p2o)  
-
-- [GitHub \- AtsushiSakai/PythonRobotics
-/SLAM/PoseOptimizationSLAM](https://github.com/AtsushiSakai/PythonRobotics/tree/master/SLAM/PoseOptimizationSLAM)
-
-- [リー代数による3次元ポーズ調整\(Pose Adjustment\)\[PythonRobotics\]\[SLAM\] \- Qiita](https://qiita.com/saitosasaki/items/a0540cba994f08bf5e16#comment-3e6588e6b096cc2567d8)
diff --git a/SLAM/PoseOptimizationSLAM3D/data_downloader.py b/SLAM/PoseOptimizationSLAM3D/data_downloader.py
deleted file mode 100644
index a0c04fd1..00000000
--- a/SLAM/PoseOptimizationSLAM3D/data_downloader.py
+++ /dev/null
@@ -1,26 +0,0 @@
-"""
-
-Data down loader for pose optimization
-
-author: Atsushi Sakai
-
-"""
-
-import subprocess
-
-
-def main():
-    print("start!!")
-
-    cmd = "wget https://www.dropbox.com/s/zu23p8d522qccor/parking-garage.g2o?dl=0 -O parking-garage.g2o -nc"
-    subprocess.call(cmd, shell=True)
-    cmd = "wget http://www.furo.org/irie/datasets/torus3d_guess.g2o -nc"
-    subprocess.call(cmd, shell=True)
-    cmd = "wget http://www.furo.org/irie/datasets/sphere2200_guess.g2o -nc"
-    subprocess.call(cmd, shell=True)
-
-    print("done!!")
-
-
-if __name__ == '__main__':
-    main()
diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py
index 924619fd..3512ef97 100644
--- a/SLAM/iterative_closest_point/iterative_closest_point.py
+++ b/SLAM/iterative_closest_point/iterative_closest_point.py
@@ -1,25 +1,26 @@
 """
 Iterative Closest Point (ICP) SLAM example
-author: Atsushi Sakai (@Atsushi_twi)
+author: Atsushi Sakai (@Atsushi_twi), Göktuğ Karakaşlı
 """
 
 import math
-import numpy as np
+
 import matplotlib.pyplot as plt
+import numpy as np
 
 #  ICP parameters
 EPS = 0.0001
-MAXITER = 100
+MAX_ITER = 100
 
 show_animation = True
 
 
-def ICP_matching(ppoints, cpoints):
+def icp_matching(previous_points, current_points):
     """
     Iterative Closest Point matching
     - input
-    ppoints: 2D points in the previous frame
-    cpoints: 2D points in the current frame
+    previous_points: 2D points in the previous frame
+    current_points: 2D points in the current frame
     - output
     R: Rotation matrix
     T: Translation vector
@@ -35,17 +36,17 @@ def ICP_matching(ppoints, cpoints):
 
         if show_animation:  # pragma: no cover
             plt.cla()
-            plt.plot(ppoints[0, :], ppoints[1, :], ".r")
-            plt.plot(cpoints[0, :], cpoints[1, :], ".b")
+            plt.plot(previous_points[0, :], previous_points[1, :], ".r")
+            plt.plot(current_points[0, :], current_points[1, :], ".b")
             plt.plot(0.0, 0.0, "xr")
             plt.axis("equal")
             plt.pause(0.1)
 
-        inds, error = nearest_neighbor_assosiation(ppoints, cpoints)
-        Rt, Tt = SVD_motion_estimation(ppoints[:, inds], cpoints)
+        indexes, error = nearest_neighbor_association(previous_points, current_points)
+        Rt, Tt = svd_motion_estimation(previous_points[:, indexes], current_points)
 
         # update current points
-        cpoints = (Rt @ cpoints) + Tt[:, np.newaxis]
+        current_points = (Rt @ current_points) + Tt[:, np.newaxis]
 
         H = update_homogeneous_matrix(H, Rt, Tt)
 
@@ -56,7 +57,7 @@ def ICP_matching(ppoints, cpoints):
         if dError <= EPS:
             print("Converge", error, dError, count)
             break
-        elif MAXITER <= count:
+        elif MAX_ITER <= count:
             print("Not Converge...", error, dError, count)
             break
 
@@ -85,31 +86,29 @@ def update_homogeneous_matrix(Hin, R, T):
         return Hin @ H
 
 
-def nearest_neighbor_assosiation(ppoints, cpoints):
+def nearest_neighbor_association(previous_points, current_points):
 
     # calc the sum of residual errors
-    dcpoints = ppoints - cpoints
-    d = np.linalg.norm(dcpoints, axis=0)
+    delta_points = previous_points - current_points
+    d = np.linalg.norm(delta_points, axis=0)
     error = sum(d)
 
     # calc index with nearest neighbor assosiation
-    d = np.linalg.norm(
-            np.repeat(cpoints, ppoints.shape[1], axis=1) - np.tile(ppoints, (1,
-                cpoints.shape[1])), axis=0)
-    inds = np.argmin(d.reshape(cpoints.shape[1], ppoints.shape[1]), axis=1)
+    d = np.linalg.norm(np.repeat(current_points, previous_points.shape[1], axis=1)
+                       - np.tile(previous_points, (1, current_points.shape[1])), axis=0)
+    indexes = np.argmin(d.reshape(current_points.shape[1], previous_points.shape[1]), axis=1)
 
-    return inds, error
+    return indexes, error
 
 
-def SVD_motion_estimation(ppoints, cpoints):
+def svd_motion_estimation(previous_points, current_points):
+    pm = np.mean(previous_points, axis=1)
+    cm = np.mean(current_points, axis=1)
 
-    pm = np.mean(ppoints, axis=1)
-    cm = np.mean(cpoints, axis=1)
+    p_shift = previous_points - pm[:, np.newaxis]
+    c_shift = current_points - cm[:, np.newaxis]
 
-    pshift = ppoints - pm[:, np.newaxis]
-    cshift = cpoints - cm[:, np.newaxis]
-
-    W = cshift @ pshift.T
+    W = c_shift @ p_shift.T
     u, s, vh = np.linalg.svd(W)
 
     R = (u @ vh).T
@@ -133,16 +132,16 @@ def main():
         # previous points
         px = (np.random.rand(nPoint) - 0.5) * fieldLength
         py = (np.random.rand(nPoint) - 0.5) * fieldLength
-        ppoints = np.vstack((px, py))
+        previous_points = np.vstack((px, py))
 
         # current points
         cx = [math.cos(motion[2]) * x - math.sin(motion[2]) * y + motion[0]
               for (x, y) in zip(px, py)]
         cy = [math.sin(motion[2]) * x + math.cos(motion[2]) * y + motion[1]
               for (x, y) in zip(px, py)]
-        cpoints = np.vstack((cx, cy))
+        current_points = np.vstack((cx, cy))
 
-        R, T = ICP_matching(ppoints, cpoints)
+        R, T = icp_matching(previous_points, current_points)
         print("R:", R)
         print("T:", T)
 
diff --git a/docs/modules/Kalmanfilter_basics.rst b/docs/modules/Kalmanfilter_basics.rst
index ce84fe0d..7325dafd 100644
--- a/docs/modules/Kalmanfilter_basics.rst
+++ b/docs/modules/Kalmanfilter_basics.rst
@@ -180,7 +180,7 @@ Central Limit Theorem
 ^^^^^^^^^^^^^^^^^^^^^
 
 According to this theorem, the average of n samples of random and
-independant variables tends to follow a normal distribution as we
+independent variables tends to follow a normal distribution as we
 increase the sample size.(Generally, for n>=30)
 
 .. code-block:: ipython3
diff --git a/tests/test_LQR_planner.py b/tests/test_LQR_planner.py
index 5ece27f8..2bcf828c 100644
--- a/tests/test_LQR_planner.py
+++ b/tests/test_LQR_planner.py
@@ -1,6 +1,6 @@
+import sys
 from unittest import TestCase
 
-import sys
 sys.path.append("./PathPlanning/LQRPlanner")
 
 from PathPlanning.LQRPlanner import LQRplanner as m
@@ -11,5 +11,5 @@ print(__file__)
 class Test(TestCase):
 
     def test1(self):
-        m.show_animation = False
+        m.SHOW_ANIMATION = False
         m.main()