diff --git a/rb_ws/src/buggy/launch/sim_2d_single.launch b/rb_ws/src/buggy/launch/sim_2d_single.launch
index 389a693..6848016 100755
--- a/rb_ws/src/buggy/launch/sim_2d_single.launch
+++ b/rb_ws/src/buggy/launch/sim_2d_single.launch
@@ -1,6 +1,6 @@
 <launch>
     <arg name="start_pos"        default="Hill1_SC" />
-    <arg name="autonsystem_args" default="--controller mpc --dist 0.0 --traj frew_test.json --self_name SC" />
+    <arg name="autonsystem_args" default="--controller mpc --dist 0.0 --traj buggycourse_safe_1.json --self_name SC" />
     <arg name="velocity"         default="15.0" />
     <arg name="buggy_name"       default="SC" />
 
diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py
index d2160a7..8ae9fe8 100755
--- a/rb_ws/src/buggy/scripts/auton/autonsystem.py
+++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py
@@ -4,7 +4,6 @@
 from threading import Lock
 
 import threading
-from rb_ws.src.buggy.scripts.auton.buggystate import BuggyState
 import rospy
 
 # ROS Message Imports
@@ -16,11 +15,8 @@
 from trajectory import Trajectory
 from world import World
 from controller import Controller
-from pure_pursuit_controller import PurePursuitController
 from stanley_controller import StanleyController
-from brake_controller import BrakeController
 from model_predictive_controller import ModelPredictiveController
-# from model_predictive_interpolation import ModelPredictiveController
 from path_planner import PathPlanner
 from pose import Pose
 
@@ -38,7 +34,6 @@ class AutonSystem:
 
     global_trajectory: Trajectory = None
     local_controller: Controller = None
-    brake_controller: BrakeController = None
     lock = None
     steer_publisher = None
     ticks = 0
@@ -46,7 +41,6 @@ class AutonSystem:
     def __init__(self,
             global_trajectory,
             local_controller,
-            brake_controller,
             self_name,
             other_name,
             curb_traj,
@@ -58,10 +52,9 @@ def __init__(self,
         # local trajectory is initialized as global trajectory. If there is no other buggy,
         # the local trajectory is never updated.
 
-        self.has_other_buggy = not other_name is None
+        self.has_other_buggy = not (other_name is None)
         self.cur_traj = global_trajectory
         self.local_controller = local_controller
-        self.brake_controller = brake_controller
 
         left_curb = curb_traj
         self.path_planner = PathPlanner(global_trajectory, left_curb)
@@ -70,13 +63,14 @@ def __init__(self,
         self.lock = Lock()
         self.ticks = 0
         self.self_odom_msg = None
-        # self.gps_odom_msg = None
+        self.gps_odom_msg = None
         self.other_odom_msg = None
-        # self.use_gps_pos = False
+        self.use_gps_pos = False
 
         rospy.Subscriber(self_name + "/nav/odom", Odometry, self.update_self_odom)
         rospy.Subscriber(self_name + "/gnss1/odom", Odometry, self.update_self_odom_backup)
-        # only if the filtered position has separated do we use the antenna position
+
+        # to report if the filter position has separated (so we need to use the antenna position)
         rospy.Subscriber(self_name + "/debug/filter_gps_seperation_status", Bool, self.update_use_gps)
 
         if self.has_other_buggy:
@@ -91,9 +85,6 @@ def __init__(self,
         self.steer_publisher = rospy.Publisher(
             self_name + "/buggy/input/steering", Float64, queue_size=1
         )
-        self.brake_debug_publisher = rospy.Publisher(
-            self_name + "/auton/debug/brake", Float64, queue_size=1
-        )
         self.heading_publisher = rospy.Publisher(
             self_name + "/auton/debug/heading", Float32, queue_size=1
         )
@@ -101,7 +92,6 @@ def __init__(self,
             self_name + "/auton/debug/distance", Float64, queue_size=1
         )
 
-
         self.controller_rate = 100
         self.rosrate_controller = rospy.Rate(self.controller_rate)
 
@@ -111,13 +101,15 @@ def __init__(self,
         self.profile = profile
         self.tick_caller()
 
+
+    # functions to read data from ROS nodes
     def update_use_gps(self, msg):
         with self.lock:
             self.use_gps_pos = msg.data
 
-    # def update_self_odom_backup(self, msg):
-    #     with self.lock:
-    #         self.gps_odom_msg = msg
+    def update_self_odom_backup(self, msg):
+        with self.lock:
+            self.gps_odom_msg = msg
 
     def update_self_odom(self, msg):
         with self.lock:
@@ -131,24 +123,32 @@ def update_other_steering_angle(self, msg):
         with self.lock:
             self.other_steering = msg.data
 
+
     def init_check(self):
-        # checks that messages are being receieved
-        # (from both buggies if relevant)
-        # covariance is less than 1 meter
+        """
+        Checks if it's safe to switch the buggy into autonomous driving mode.
+        Specifically, it checks:
+            if we can recieve odometry messages from the buggy
+            if the covariance is acceptable (less than 1 meter)
+            if the buggy thinks it is facing in the correct direction wrt the local trajectory (not 180 degrees flipped)
+
+        Returns:
+           A boolean describing the status of the buggy (safe for auton or unsafe for auton)
+        """
+        # TODO: should we check if we're recieving messages from NAND?
         if (self.self_odom_msg == None):
             rospy.logwarn("WARNING: no available position estimate")
             return False
 
-        if (self.sc_state.get_pos_covariance[0] ** 2 + self.sc_state.get_pos_covariance[7] ** 2 > 1**2):
+        if (self.self_odom_msg.pose.covariance[0] ** 2 + self.self_odom_msg.pose.covariance[7] ** 2 > 1**2):
             rospy.logwarn("checking position estimate certainty")
             return False
 
         # waits until covariance is acceptable to check heading
         with self.lock:
-            self_easting, self_northing, self_yaw = self.sc_state.get_pose()
-            current_heading = self_yaw
-            # TODO: make get_closest_index_on_path work with UTM, not world!
-            closest_heading = self.cur_traj.get_heading_by_index(trajectory.get_closest_index_on_path(self_easting, self_northing))
+            self_pose = self.get_world_pose(self.self_odom_msg)
+            current_heading = self_pose.theta
+            closest_heading = self.cur_traj.get_heading_by_index(trajectory.get_closest_index_on_path(self_pose.x, self_pose.y))
         rospy.loginfo("current heading: " + str(np.rad2deg(current_heading)))
         self.heading_publisher.publish(Float32(np.rad2deg(current_heading)))
 
@@ -161,14 +161,15 @@ def init_check(self):
             closest_heading = 2*np.pi + closest_heading
 
         if (abs(current_heading - closest_heading) >= np.pi/2):
-            rospy.logwarn("WARNING: INCORRECT HEADING! restart stack. Current heading [-180, 180]: " + str(np.rad2deg(self_yaw)))
+            rospy.logwarn("WARNING: INCORRECT HEADING! restart stack. Current heading [-180, 180]: " + str(np.rad2deg(self_pose.theta)))
             return False
 
         return True
 
     def tick_caller(self):
-
-
+        """
+        The main scheduler - starts threads for the pathplanner and the controller.
+        """
         rospy.loginfo("start checking initialization status")
         while ((not rospy.is_shutdown()) and not self.init_check()):
             self.init_check_publisher.publish(False)
@@ -195,8 +196,8 @@ def tick_caller(self):
             t_planner.join()
 
     def get_world_pose(self, msg):
+        #TODO: this should be redundant - converting rospose to pose should automatically handle world conversions
         current_rospose = msg.pose.pose
-
         # Get data from message
         pose_gps = Pose.rospose_to_pose(current_rospose)
         return World.gps_to_world_pose(pose_gps)
@@ -207,15 +208,20 @@ def local_controller_thread(self):
             self.rosrate_controller.sleep()
 
     def local_controller_tick(self):
+        if not self.use_gps_pos:
+            with self.lock:
+                state_msg = self.self_odom_msg
+        else:
+            with self.lock:
+                state_msg = self.gps_odom_msg
 
         # For viz and debugging
-        sc_easting, sc_northing, sc_yaw,  = self.sc_state.get_pose()
-        self.heading_publisher.publish(Float32(np.rad2deg(sc_yaw)))
+        pose = self.get_world_pose(state_msg)
+        self.heading_publisher.publish(Float32(np.rad2deg(pose.theta)))
 
         # Compute control output
-        # TODO: rewrite compute_control to use buggy state, NOT the entire odom object. (why were we passing around an odom object anyway? instead of just subscribing to it?)
         steering_angle = self.local_controller.compute_control(
-            self.sc_state, self.cur_traj)
+            state_msg, self.cur_traj)
         steering_angle_deg = np.rad2deg(steering_angle)
         self.steer_publisher.publish(Float64(steering_angle_deg))
 
@@ -223,35 +229,38 @@ def local_controller_tick(self):
     def planner_thread(self):
         while (not rospy.is_shutdown()):
             self.rosrate_planner.sleep()
-            if not self.nand_state.get_pose is None:
+            if not self.other_odom_msg is None:
                 with self.lock:
-                    sc_easting, sc_northing, sc_yaw = self.sc_state.get_pose()
-                    nand_easting, nand_northing, nand_yaw = self.nand_state.get_pose()
+                    self_pose = self.get_world_pose(self.self_odom_msg)
+                    other_pose = self.get_world_pose(self.other_odom_msg)
 
-                distance = (sc_easting - nand_easting) ** 2 + (sc_northing - nand_northing) ** 2
+                distance = (self_pose.x - other_pose.x) ** 2 + (self_pose.y - other_pose.y) ** 2
                 distance = np.sqrt(distance)
                 self.distance_publisher.publish(Float64(distance))
 
                 self.planner_tick()
 
     def planner_tick(self):
-        # NOTE: do we want to check for None?
-        with self.lock:
-            # NOTE: these are in UTM (used to be in world)
-            sc_easting, sc_northing, sc_yaw = self.sc_state.get_pose()
+        if not self.use_gps_pos:
+            with self.lock:
+                self_pose = self.get_world_pose(self.self_odom_msg)
+        else:
+            with self.lock:
+                self_pose = self.get_world_pose(self.gps_odom_msg)
 
         with self.lock:
-            nand_easting, nand_northing, nand_yaw = self.nand_state.get_pose()
+            other_pose = self.get_world_pose(self.other_odom_msg)
 
         # update local trajectory via path planner
-        # TODO: have compute_traj take in a buggy state instead of poses
         self.cur_traj, cur_idx = self.path_planner.compute_traj(
-                                            self.sc_state,
-                                            self.nand_state)
+                                            self_pose,
+                                            other_pose)
         self.local_controller.current_traj_index = cur_idx
 
-if __name__ == "__main__":
-    rospy.init_node("auton_system")
+def init_parser ():
+    """
+        Returns a parser to read launch file arguments to AutonSystem.
+    """
     parser = argparse.ArgumentParser()
     parser.add_argument("--controller",
         type=str,
@@ -293,6 +302,11 @@ def planner_tick(self):
         "--profile",
         action='store_true',
         help="turn on profiling for the path planner")
+    return parser
+
+if __name__ == "__main__":
+    rospy.init_node("auton_system")
+    parser = init_parser()
 
     args, _ = parser.parse_known_args()
     ctrl = args.controller
@@ -321,12 +335,6 @@ def planner_tick(self):
         local_ctrller = StanleyController(
             self_name,
             start_index=start_index)
-
-    elif (ctrl == "pure_pursuit"):
-        local_ctrller = PurePursuitController(
-            self_name,
-            start_index=start_index)
-
     elif (ctrl == "mpc"):
         local_ctrller = ModelPredictiveController(
             self_name,
@@ -334,10 +342,10 @@ def planner_tick(self):
 
     if (local_ctrller == None):
         raise Exception("Invalid Controller Argument")
+
     auton_system = AutonSystem(
         trajectory,
         local_ctrller,
-        BrakeController(),
         self_name,
         other_name,
         left_curb,
@@ -345,4 +353,4 @@ def planner_tick(self):
     )
 
     while not rospy.is_shutdown():
-        rospy.spin()
+        rospy.spin()
\ No newline at end of file
diff --git a/rb_ws/src/buggy/scripts/auton/world.py b/rb_ws/src/buggy/scripts/auton/world.py
new file mode 100644
index 0000000..9f26d6c
--- /dev/null
+++ b/rb_ws/src/buggy/scripts/auton/world.py
@@ -0,0 +1,226 @@
+import utm
+import numpy as np
+
+from pose import Pose
+
+
+class World:
+    """Abstraction for the world coordinate system
+
+    The real world uses GPS coordinates, aka latitude and longitude. However,
+    using lat/lon is bad for path planning for several reasons. First, the difference
+    in numbers would be very tiny for small distances, alluding to roundoff errors.
+    Additionally, lat/lon follows a North-East-Down coordinate system, with headings
+    in the clockwise direction. We want to use an East-North-Up coordinate system, so
+    that the heading is in the counter-clockwise direction.
+
+    We do this by converting GPS coordinates to UTM coordinates, which are in meters.
+    UTM works by dividing the world into a grid, where each grid cell has a unique
+    identifier. A UTM coordinate consists of the grid cell identifier and the "easting"
+    and "northing" within that grid cell. The easting (x) and northing (y) are in meters,
+    and are relative to the southwest corner of the grid cell.
+
+    Last, we offset the UTM coordinates to be relative to some arbitrary zero point. That
+    way, the final world coordinates are relatively close to zero, which makes debugging
+    easier.
+
+    This class provides methods to convert between GPS and world coordinates. There is
+    a version for single coordinates and a version for numpy arrays.
+    """
+
+    # Geolocates to around the southwest corner of Phipps
+    WORLD_EAST_ZERO = 589106
+    WORLD_NORTH_ZERO = 4476929
+
+    @staticmethod
+    def gps_to_world(lat, lon):
+        """Converts GPS coordinates to world coordinates
+
+        Args:
+            lat (float): latitude
+            lon (float): longitude
+
+        Returns:
+            tuple: (x, y) in meters from some arbitrary zero point
+        """
+        utm_coords = utm.from_latlon(lat, lon)
+        x = utm_coords[0] - World.WORLD_EAST_ZERO
+        y = utm_coords[1] - World.WORLD_NORTH_ZERO
+
+        return x, y
+
+    @staticmethod
+    def utm_to_world_pose(pose: Pose) -> Pose:
+        """Converts UTM coordinates to world coordinates
+
+        Args:
+            pose (Pose): pose with utm coordinates
+
+        Returns:
+            Pose: pose with world coordinates
+        """
+
+        utm_e = pose.x
+        utm_n = pose.y
+        x = utm_e - World.WORLD_EAST_ZERO
+        y = utm_n - World.WORLD_NORTH_ZERO
+        return Pose(x, y, pose.theta)
+
+    @staticmethod
+    def world_to_utm_pose(pose: Pose) -> Pose:
+        """Converts world coordinates to utm coordinates
+
+        Args:
+            pose (Pose): pose with world coordinates
+
+        Returns:
+            Pose: pose with world coordinates
+        """
+
+        utm_e = pose.x + World.WORLD_EAST_ZERO
+        utm_n = pose.y + World.WORLD_NORTH_ZERO
+        return Pose(utm_e, utm_n, pose.theta)
+
+    @staticmethod
+    def world_to_utm_numpy(coords):
+        """Converts world coordinates to utm coordinates
+
+        Args:
+            coords (numpy.ndarray [size: (N,2)]): array of x, y pairs
+
+        Returns:
+            numpy.ndarray [size: (N,2)]: array of utm_e, utm_n pairs
+        """
+
+        N = np.shape(coords)[0]
+        utm_offset_e = np.ones((N, )) * World.WORLD_EAST_ZERO
+        utm_offset_n = np.ones((N, )) * World.WORLD_NORTH_ZERO
+        utm_offset = np.stack((utm_offset_e, utm_offset_n), axis=1)
+
+        return coords + utm_offset
+
+    @staticmethod
+    def utm_to_world_numpy(coords):
+        """Converts utm coordinates to world coordinates
+
+        Args:
+            coords (numpy.ndarray [size: (N,2)]): array of utm_e, utm_n pairs
+
+        Returns:
+            numpy.ndarray [size: (N,2)]: array of x, y pairs
+        """
+
+        N = np.shape(coords)[0]
+        utm_offset_e = np.ones((N, )) * World.WORLD_EAST_ZERO
+        utm_offset_n = np.ones((N, )) * World.WORLD_NORTH_ZERO
+        utm_offset = np.stack((utm_offset_e, utm_offset_n), axis=1)
+
+        return coords - utm_offset
+
+
+    @staticmethod
+    def world_to_gps(x, y):
+        """Converts world coordinates to GPS coordinates
+
+        Args:
+            x (float): x in meters from some arbitrary zero point
+            y (float): y in meters from some arbitrary zero point
+
+        Returns:
+            tuple: (lat, lon)
+        """
+        utm_coords = utm.to_latlon(
+            x + World.WORLD_EAST_ZERO, y + World.WORLD_NORTH_ZERO, 17, "T"
+        )
+        lat = utm_coords[0]
+        lon = utm_coords[1]
+
+        return lat, lon
+
+    @staticmethod
+    def utm_to_gps(x, y):
+        """Converts utm coordinates to GPS coordinates
+
+        Args:
+            x (float): x in meters, in utm frame
+            y (float): y in meters, in utm frame
+
+        Returns:
+            tuple: (lat, lon)
+        """
+        utm_coords = utm.to_latlon(
+            x, y, 17, "T"
+        )
+        lat = utm_coords[0]
+        lon = utm_coords[1]
+
+        return lat, lon
+
+    @staticmethod
+    def gps_to_world_numpy(coords):
+        """Converts GPS coordinates to world coordinates
+
+        Args:
+            coords (numpy.ndarray [size: (N,2)]): array of lat, lon pairs
+
+        Returns:
+            numpy.ndarray [size: (N,2)]: array of x, y pairs
+        """
+        utm_coords = utm.from_latlon(coords[:, 0], coords[:, 1])
+        x = utm_coords[0] - World.WORLD_EAST_ZERO
+        y = utm_coords[1] - World.WORLD_NORTH_ZERO
+
+        return np.stack((x, y), axis=1)
+
+    @staticmethod
+    def world_to_gps_numpy(coords):
+        """Converts world coordinates to GPS coordinates
+
+        Args:
+            coords (numpy.ndarray [size: (N,2)]): array of x, y pairs
+
+        Returns:
+            numpy.ndarray [size: (N,2)]: array of lat, lon pairs
+        """
+
+        # Pittsburgh is in UTM zone 17T.
+        utm_coords = utm.to_latlon(
+            coords[:, 0] + World.WORLD_EAST_ZERO,
+            coords[:, 1] + World.WORLD_NORTH_ZERO,
+            17,
+            "T",
+        )
+        lat = utm_coords[0]
+        lon = utm_coords[1]
+
+        return np.stack((lat, lon), axis=1)
+
+    @staticmethod
+    def gps_to_world_pose(pose: Pose):
+        """Converts GPS coordinates to world coordinates
+
+        Args:
+            pose (Pose): pose with lat, lon coordinates (x=lon, y=lat)
+
+        Returns:
+            Pose: pose with x, y coordinates
+        """
+        world_coords = World.gps_to_world(pose.y, pose.x)
+        new_heading = pose.theta  # INS uses ENU frame so no heading conversion needed
+
+        return Pose(world_coords[0], world_coords[1], new_heading)
+
+    @staticmethod
+    def world_to_gps_pose(pose: Pose):
+        """Converts world coordinates to GPS coordinates
+
+        Args:
+            pose (Pose): pose with x, y coordinates
+
+        Returns:
+            Pose: pose with lat, lon coordinates
+        """
+        gps_coords = World.world_to_gps(pose.x, pose.y)
+        new_heading = pose.theta
+
+        return Pose(gps_coords[1], gps_coords[0], new_heading)
\ No newline at end of file