diff --git a/reference_path.py b/reference_path.py
new file mode 100644
index 0000000..50d21cb
--- /dev/null
+++ b/reference_path.py
@@ -0,0 +1,236 @@
+import numpy as np
+import math
+from map import Map
+from bresenham import bresenham
+import matplotlib.pyplot as plt
+
+
+############
+# Waypoint #
+############
+
+class Waypoint:
+    def __init__(self, x, y, psi, kappa):
+        """
+        Waypoint object containing x, y location in global coordinate system,
+        orientation of waypoint psi and local curvature kappa
+        :param x: x position in global coordinate system | [m]
+        :param y: y position in global coordinate system | [m]
+        :param psi: orientation of waypoint | [rad]
+        :param kappa: local curvature | [1 / m]
+        """
+        self.x = x
+        self.y = y
+        self.psi = psi
+        self.kappa = kappa
+
+    def __sub__(self, other):
+        return ((self.x - other.x)**2 + (self.y - other.y)**2)**0.5
+
+
+##################
+# Reference Path #
+##################
+
+
+class ReferencePath:
+    def __init__(self, map, wp_x, wp_y, resolution, smoothing_distance, width_info=False):
+        """
+        Reference Path object. Create a reference trajectory from specified
+        corner points with given resolution. Smoothing around corners can be
+        applied.
+        """
+        # precision
+        self.eps = 1e-12
+
+        # map
+        self.map = map
+
+        # resolution of the path
+        self.resolution = resolution
+
+        # look ahead distance for path averaging
+        self.smoothing_distance = smoothing_distance
+
+        # waypoints with x, y, psi, k
+        self.waypoints = self.construct_path(wp_x, wp_y)
+
+        # path width
+        self.get_width_info = width_info
+        if self.get_width_info:
+            self.width_info = self.compute_width()
+            self.min_width = (np.min(self.width_info[0, :]),
+                                np.min(self.width_info[3, :]))
+
+    def construct_path(self, wp_x, wp_y):
+
+        # Number of waypoints
+        n_wp = [int(np.sqrt((wp_x[i + 1] - wp_x[i]) ** 2 +
+                            (wp_y[i + 1] - wp_y[i]) ** 2) /
+            self.resolution) for i in range(len(wp_x) - 1)]
+
+        # Construct waypoints with specified resolution
+        gp_x, gp_y = wp_x[-1], wp_y[-1]
+        wp_x = [np.linspace(wp_x[i], wp_x[i+1], n_wp[i], endpoint=False).
+                    tolist() for i in range(len(wp_x)-1)]
+        wp_x = [wp for segment in wp_x for wp in segment] + [gp_x]
+        wp_y = [np.linspace(wp_y[i], wp_y[i + 1], n_wp[i], endpoint=False).
+                    tolist() for i in range(len(wp_y) - 1)]
+        wp_y = [wp for segment in wp_y for wp in segment] + [gp_y]
+
+        # smooth path
+        wp_xs = []
+        wp_ys = []
+        for wp_id in range(self.smoothing_distance, len(wp_x) -
+                                                    self.smoothing_distance):
+            wp_xs.append(np.mean(wp_x[wp_id - self.smoothing_distance:wp_id
+                                            + self.smoothing_distance + 1]))
+            wp_ys.append(np.mean(wp_y[wp_id - self.smoothing_distance:wp_id
+                                            + self.smoothing_distance + 1]))
+
+        waypoints = list(zip(wp_xs, wp_ys))
+        waypoints = self.spatial_reformulation(waypoints)
+        return waypoints
+
+    def spatial_reformulation(self, waypoints):
+        """
+        Reformulate conventional waypoints (x, y) coordinates into waypoint
+        objects containing (x, y, psi, kappa)
+        :return: list of waypoint objects for entire reference path
+        """
+
+        waypoints_spatial = []
+        for wp_id in range(len(waypoints) - 1):
+
+            # get start and goal waypoints
+            current_wp = np.array(waypoints[wp_id])
+            next_wp = np.array(waypoints[wp_id + 1])
+
+            # difference vector
+            dif_ahead = next_wp - current_wp
+
+            # angle ahead
+            psi = np.arctan2(dif_ahead[1], dif_ahead[0])
+
+            # distance to next waypoint
+            dist_ahead = np.linalg.norm(dif_ahead, 2)
+
+            # get x and y coordinates of current waypoint
+            x = current_wp[0]
+            y = current_wp[1]
+
+            # first waypoint
+            if wp_id == 0:
+                kappa = 0
+            else:
+                prev_wp = np.array(waypoints[wp_id - 1])
+                dif_behind = current_wp - prev_wp
+                angle_behind = np.arctan2(dif_behind[1], dif_behind[0])
+                angle_dif = np.mod(psi - angle_behind + math.pi, 2 * math.pi) \
+                            - math.pi
+                kappa = np.abs(angle_dif / (dist_ahead + self.eps))
+
+            waypoints_spatial.append(Waypoint(x, y, psi, kappa))
+
+        return waypoints_spatial
+
+    def compute_width(self, max_dist=2.0):
+        max_dist = max_dist  # m
+        width_info = np.zeros((6, len(self.waypoints)))
+        for wp_id, wp in enumerate(self.waypoints):
+            for i, dir in enumerate(['left', 'right']):
+                # get pixel coordinates of waypoint
+                wp_x, wp_y = self.map.w2m(wp.x, wp.y)
+                # get angle orthogonal to path in current direction
+                if dir == 'left':
+                    angle = np.mod(wp.psi + math.pi / 2 + math.pi,
+                                 2 * math.pi) - math.pi
+                else:
+                    angle = np.mod(wp.psi - math.pi / 2 + math.pi,
+                                   2 * math.pi) - math.pi
+                # get closest cell to orthogonal vector
+                t_x, t_y = self.map.w2m(wp.x + max_dist * np.cos(angle), wp.y + max_dist * np.sin(angle))
+                # compute path between cells
+                width_info[3*i:3*(i+1), wp_id] = self.get_min_dist(wp_x, wp_y, t_x, t_y, max_dist)
+        return width_info
+
+    def get_min_dist(self, wp_x, wp_y, t_x, t_y, max_dist):
+        # get neighboring cells (account for discretization)
+        neighbors_x, neighbors_y = [], []
+        for i in range(-1, 2, 1):
+            for j in range(-1, 2, 1):
+                neighbors_x.append(t_x + i)
+                neighbors_y.append(t_y + j)
+
+        # get bresenham paths to all neighboring cells
+        paths = []
+        for t_x, t_y in zip(neighbors_x, neighbors_y):
+            path = list(bresenham(wp_x, wp_y, t_x, t_y))
+            paths.append(path)
+
+        min_dist = max_dist
+        min_cell = self.map.m2w(t_x, t_y)
+        for path in paths:
+            for cell in path:
+                t_x = cell[0]
+                t_y = cell[1]
+                # if path goes through occupied cell
+                if self.map.data[t_y, t_x] == 0:
+                    # get world coordinates
+                    x, y = self.map.m2w(wp_x, wp_y)
+                    c_x, c_y = self.map.m2w(t_x, t_y)
+                    cell_dist = np.sqrt((x - c_x) ** 2 + (y - c_y) ** 2)
+                    if cell_dist < min_dist:
+                        min_dist = cell_dist
+                        min_cell = (c_x, c_y)
+        dist_info = np.array([min_dist, min_cell[0], min_cell[1]])
+        return dist_info
+
+    def show(self):
+
+        # plot map
+        plt.clf()
+        plt.imshow(np.flipud(self.map.data),cmap='gray',
+                   extent=[self.map.origin[0], self.map.origin[0] +
+                           self.map.width * self.map.resolution,
+                           self.map.origin[1], self.map.origin[1] +
+                           self.map.height * self.map.resolution])
+        # plot reference path
+        wp_x = np.array([wp.x for wp in self.waypoints])
+        wp_y = np.array([wp.y for wp in self.waypoints])
+        plt.scatter(wp_x, wp_y, color='k', s=5)
+
+        if self.get_width_info:
+            print('Min Width Left: {:f} m'.format(self.min_width[0]))
+            print('Min Width Right: {:f} m'.format(self.min_width[1]))
+            plt.quiver(wp_x, wp_y, self.width_info[1, :] - wp_x,
+                       self.width_info[2, :] - wp_y, scale=1, units='xy',
+                       width=0.05, color='#D4AC0D')
+            plt.quiver(wp_x, wp_y, self.width_info[4, :] - wp_x,
+                       self.width_info[5, :] - wp_y, scale=1, units='xy',
+                       width=0.05, color='#BA4A00')
+
+
+if __name__ == '__main__':
+
+    # Create Map
+    map = Map(file_path='map_race.png', origin=[-1, -2], resolution=0.005)
+
+    # Specify waypoints
+    # Floor 2
+    # wp_x = [-9.169, -2.7, 11.9, 7.3, -6.95]
+    # wp_y = [-15.678, -7.12, 10.9, 14.5, -3.31]
+    # Race Track
+    wp_x = [-0.75, -0.25, -0.25, 0.25, 0.25, 1.25, 1.25, 0.75, 0.75, 1.25, 1.25, -0.75, -0.75, -0.25]
+    wp_y = [-1.5, -1.5, -0.5, -0.5, -1.5, -1.5, -1, -1, -0.5, -0.5, 0, 0, -1.5, -1.5]
+    # Specify path resolution
+    path_resolution = 0.05  # m / wp
+
+    # Smooth Path
+    reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
+                                          smoothing_distance=5)
+    reference_path.show()
+    plt.show()
+
+
+