diff --git a/config/esw.yaml b/config/esw.yaml
index f0a78c30f..cff8e0030 100644
--- a/config/esw.yaml
+++ b/config/esw.yaml
@@ -1,5 +1,5 @@
 imu_driver:
-  port: "/dev/imu"
+  port: "/dev/tty.usbmodem101"
   baud: 115200
   frame_id: "imu_link"
 
diff --git a/launch/auton.launch b/launch/auton.launch
new file mode 100644
index 000000000..3d63c02b6
--- /dev/null
+++ b/launch/auton.launch
@@ -0,0 +1,51 @@
+<!-- This launch file launches all nodes necessary for autonomous navigation. -->
+<launch>
+  <arg name="run_tag_detector" default="true"/>
+  <arg name="sim" default="false"/>
+  <arg name="use_ekf" default="true"/>
+  <arg name="ekf_start_delay" default="0"/>
+
+  <!--
+    ==========
+    Perception
+    ==========
+  -->
+  <node pkg="nodelet" type="nodelet" name="perception_nodelet_manager" respawn="true"
+        args="manager" output="screen"/>
+  <!-- nodelet to detect AR tags and publish them to the TF tree -->
+  <node if="$(arg run_tag_detector)"
+        pkg="nodelet" type="nodelet" name="tag_detector" respawn="true"
+        args="load mrover/TagDetectorNodelet perception_nodelet_manager" output="screen"/>
+
+  <!--
+    ===========
+    Navigation
+    ===========
+  -->
+  <!-- node to navigate to a series of waypoints, AR tags, and gates -->
+  <rosparam command="load" file="$(find mrover)/config/navigation.yaml"/>
+  <node name="nav" pkg="mrover" type="navigation.py"/>
+  <node name="failure_id" pkg="mrover" type="failure_identification.py"/>
+
+  <!--
+    ============
+    Localization
+    ============
+  -->
+  <rosparam command="load" file="$(find mrover)/config/localization.yaml"/>
+  <rosparam if="$(arg sim)" command="load" file="$(find mrover)/config/sim_ekf.yaml"/>
+  <rosparam unless="$(arg sim)" command="load" file="$(find mrover)/config/ekf.yaml"/>
+
+  <!-- global EKF -->
+  <node if="$(arg use_ekf)" pkg="robot_localization" type="ekf_localization_node" name="global_ekf"
+        clear_params="true" launch-prefix="bash -c 'sleep $(arg ekf_start_delay); $0 $@'">
+    <remap from="odometry/filtered" to="global_ekf/odometry"/>
+  </node>
+
+  <!-- passthrough filter to replace EKF in datapath -->
+  <node unless="$(arg use_ekf)" pkg="mrover" type="passthrough_filter.py" name="passthrough_filter"/>
+
+  <!-- node to linearize GPS from geodetic to ENU cartesian coords -->
+  <node name="gps_linearization" pkg="mrover" type="gps_linearization.py" output="screen"/>
+  <node name="tip_detection" pkg="mrover" type="tip_detection.py" output="screen" />
+</launch>
diff --git a/launch/auton_sim.launch b/launch/auton_sim.launch
new file mode 100644
index 000000000..49b224d10
--- /dev/null
+++ b/launch/auton_sim.launch
@@ -0,0 +1,27 @@
+<!-- 
+    This launch file launches everything necessary to run auton code in the simulator.
+ -->
+<launch>
+    <arg name="rvizconfig" default="$(find mrover)/config/rviz/auton_sim.rviz"/>
+    <arg name="world_name" default="world"/>
+
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true"/>
+
+    <!-- launch rover core nodes -->
+    <include file="$(find mrover)/launch/rover_core.launch"/>
+
+    <!-- launch the simulator -->
+    <include file="$(find mrover)/launch/simulator.launch">
+        <arg name="world_name" value="$(arg world_name)"/>
+    </include>
+
+    <node pkg="tf" type="static_transform_publisher" name="zed2i_left_camera_frame_publisher"
+          args="0 0 0 0 0 0 1 left_camera_link zed2i_left_camera_frame 100"/>
+
+    <!-- launch auton -->
+    <include file="$(find mrover)/launch/auton.launch">
+        <arg name="sim" value="true"/>
+        <arg name="use_ekf" value="true"/>
+        <arg name="ekf_start_delay" value="5"/>
+    </include>
+</launch>
diff --git a/launch/localization.launch b/launch/localization.launch
index f9491cafa..2f14c13ae 100644
--- a/launch/localization.launch
+++ b/launch/localization.launch
@@ -30,4 +30,6 @@
         <node unless="$(arg sim)" name="gps_driver" pkg="mrover" type="gps_driver.py" output="screen" />
     </group>
 
+    <node name="tip_detection" pkg="mrover" type="tip_detection.py" output="screen" />
+
 </launch>
\ No newline at end of file
diff --git a/src/esw/imu_driver.py b/src/esw/imu_driver.py
index 1921e8bef..6b7bde5de 100755
--- a/src/esw/imu_driver.py
+++ b/src/esw/imu_driver.py
@@ -4,7 +4,7 @@
 from sensor_msgs.msg import Temperature, Imu, MagneticField
 from geometry_msgs.msg import Quaternion, Vector3, PoseWithCovarianceStamped, PoseWithCovariance, Vector3Stamped, Pose
 from std_msgs.msg import Header
-from mrover.msg import CalibrationStatus, ImuAndMag
+from mrover.msg import ImuAndMag, CalibrationStatus
 from tf.transformations import quaternion_about_axis, quaternion_multiply, rotation_matrix, quaternion_from_matrix
 from typing import Tuple, List
 from copy import deepcopy
diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
new file mode 100755
index 000000000..a6d6415a9
--- /dev/null
+++ b/src/localization/tip_detection.py
@@ -0,0 +1,85 @@
+#!/usr/bin/env python3
+
+import tf2_ros
+import numpy as np
+import rospy
+from std_msgs.msg import Bool
+from util.SE3 import SE3
+import time
+
+
+class TipDetection:
+    hit_count: int
+    orientation_threshold: float
+    hit_count_threshold: int
+    reset_hit_count_threshold: int
+    time_counter: time
+    current_time: time
+    tip_publisher: rospy.Publisher
+
+    def __init__(self):
+        self.tip_publisher = rospy.Publisher("tipping", Bool, queue_size=1)
+
+        self.hit_count = 0
+        self.orientation_threshold = 0.8
+        self.hit_count_threshold = 10
+        self.reset_hit_count_threshold = 3
+        self.time_counter = time.time()
+        self.current_time = time.time()
+
+        self.buffer = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.buffer)
+        self.world_frame = rospy.get_param("world_frame")
+        self.rover_frame = rospy.get_param("rover_frame")
+        time.sleep(2)
+        self.in_loop()
+
+    def in_loop(self):
+        rate = rospy.Rate(10.0)
+        while not rospy.is_shutdown():
+            try:
+                # extract yaw
+                self.old = SE3.from_tf_tree(self.buffer, self.world_frame, self.rover_frame).rotation.rotation_matrix()
+
+                # multiply yaw by the z vector [0, 0, 1] to get new transform
+                self.transform = np.dot(np.array([0, 0, 1]), self.old)
+
+                # compare this new transform with our threshold to see if it's tipping, if so increment hit_count
+                if self.transform[2] <= self.orientation_threshold:
+                    self.hit_count += 1
+
+                self.check_for_hit_count(self.hit_count)
+
+            except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
+                print(e)
+                rate.sleep()
+            # reset the hit count time
+            self.current_time = time.time()
+            self.reset_hit_count_time(self.reset_hit_count_threshold, self.time_counter)
+
+    # check if hit_count is too big
+    def check_for_hit_count(self, hit_count):
+        # if hit_count exceeds threshold
+        if hit_count > self.hit_count_threshold:
+            # publishing into tip_publisher that rover is tipping, True
+            self.tip_publisher.publish(True)
+        else:  # else publish False
+            self.tip_publisher.publish(False)
+
+    # reset hit_count each reset_hit_count_threshold seconds
+    def reset_hit_count_time(self, reset_hit_count_threshold, time_counter):
+        # if the amount of time that's passed since last reset > threshold, reset hit_count
+        if time.time() - self.time_counter > self.reset_hit_count_threshold:
+            self.hit_count = 0
+            self.time_counter = time.time()
+
+
+def main():
+    rospy.init_node("tip_detection")
+    TipDetection()
+
+    rospy.spin()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/starter_project/autonomy/AutonomyStarterProject.cmake b/starter_project/autonomy/AutonomyStarterProject.cmake
index 4b84c75af..d8873b08a 100644
--- a/starter_project/autonomy/AutonomyStarterProject.cmake
+++ b/starter_project/autonomy/AutonomyStarterProject.cmake
@@ -8,7 +8,7 @@ add_message_files(
         DIRECTORY
         ${CMAKE_CURRENT_LIST_DIR}/msg
         FILES
-        #add new messages here
+        StarterProjectTag.msg
 )
 
 # Collect all cpp files in the src subdirectory to be used for perception
diff --git a/starter_project/autonomy/launch/starter_project.launch b/starter_project/autonomy/launch/starter_project.launch
index d169e1c61..f130a0d34 100644
--- a/starter_project/autonomy/launch/starter_project.launch
+++ b/starter_project/autonomy/launch/starter_project.launch
@@ -29,7 +29,7 @@
     Perception
     ==========
   -->
-     <node name="starter_project_perception" pkg="mrover" type="starter_project_perception" />
+     <!-- <node name="starter_project_perception" pkg="mrover" type="starter_project_perception" /> -->
 
   <!--
     ===========
diff --git a/starter_project/autonomy/msg/StarterProjectTag.msg b/starter_project/autonomy/msg/StarterProjectTag.msg
new file mode 100644
index 000000000..fb3b6f843
--- /dev/null
+++ b/starter_project/autonomy/msg/StarterProjectTag.msg
@@ -0,0 +1,4 @@
+int32 tagId
+float32 xTagCenterPixel
+float32 yTagCenterPixel
+float32 closenessMetric
\ No newline at end of file
diff --git a/starter_project/autonomy/src/localization.py b/starter_project/autonomy/src/localization.py
index b8ff5423a..d34365801 100755
--- a/starter_project/autonomy/src/localization.py
+++ b/starter_project/autonomy/src/localization.py
@@ -20,6 +20,8 @@ class Localization:
     def __init__(self):
         # create subscribers for GPS and IMU data, linking them to our callback functions
         # TODO
+        rospy.Subscriber("imu/imu_only", Imu, self.imu_callback)
+        rospy.Subscriber("gps/fix", NavSatFix, self.gps_callback)
 
         # create a transform broadcaster for publishing to the TF tree
         self.tf_broadcaster = tf2_ros.TransformBroadcaster()
@@ -35,6 +37,11 @@ def gps_callback(self, msg: NavSatFix):
         that pose to the TF tree.
         """
         # TODO
+        ref = np.array([(42.293195), (-83.7096706)])
+        point = np.array([(msg.latitude), (msg.longitude)])
+        cartesian = self.spherical_to_cartesian(point, ref)
+        self.pose = SE3(cartesian, self.pose.rotation)
+        self.pose.publish_to_tf_tree(self.tf_broadcaster, "map", "base_link")
 
     def imu_callback(self, msg: Imu):
         """
@@ -43,6 +50,8 @@ def imu_callback(self, msg: Imu):
         store that value in `self.pose`, then publish that pose to the TF tree.
         """
         # TODO
+        self.pose = SE3.from_pos_quat(self.pose.position, np.array([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]))
+        self.pose.publish_to_tf_tree(sefl.tf_broadcaster, "map", "base_link")
 
     @staticmethod
     def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndarray) -> np.ndarray:
@@ -57,6 +66,11 @@ def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndar
         :returns: the approximated cartesian coordinates in meters, given as a numpy array [x, y, z]
         """
         # TODO
+        r = 6371000
+        x = (r* (np.radians(spherical_coord[1]) - np.radians(reference_coord[1])) * np.cos(np.radians(reference_coord[0])))
+        y = r * (np.radians(spherical_coord[0]) - np.radians(reference_coord[0]))
+        z = 0
+        return np.array([x, y, z])
 
 
 def main():
diff --git a/starter_project/autonomy/src/navigation/context.py b/starter_project/autonomy/src/navigation/context.py
index 4bbd06f61..b679a2ac6 100644
--- a/starter_project/autonomy/src/navigation/context.py
+++ b/starter_project/autonomy/src/navigation/context.py
@@ -18,15 +18,18 @@ class Rover:
 
     def get_pose(self) -> Optional[SE3]:
         # TODO: return the pose of the rover (or None if we don't have one (catch exception))
-        pass
+        try:
+            return SE3.SE3.from_tf_tree(self.ctx.tf_buffer, "map", "base_link")
+        except:
+            return None
 
     def send_drive_command(self, twist: Twist):
         # TODO: send the Twist message to the rover
-        pass
+        self.ctx.vel_cmd_publisher.publish(twist)
 
     def send_drive_stop(self):
         # TODO: tell the rover to stop
-        pass
+        self.send_drive_command(Twist())
 
 
 @dataclass
@@ -41,7 +44,7 @@ class Environment:
 
     def receive_fid_data(self, message: StarterProjectTag):
         # TODO: handle incoming FID data messages here
-        pass
+        self.fid_pos = message
 
     def get_fid_data(self) -> Optional[StarterProjectTag]:
         """
@@ -49,7 +52,9 @@ def get_fid_data(self) -> Optional[StarterProjectTag]:
         if it exists, otherwise returns None
         """
         # TODO: return either None or your position message
-        pass
+        if (self.fid_pos) {
+            return self.fid_pos
+        } else {return None}
 
 
 class Context:
diff --git a/starter_project/autonomy/src/navigation/drive_state.py b/starter_project/autonomy/src/navigation/drive_state.py
index bae1d07f1..3c35f2efc 100644
--- a/starter_project/autonomy/src/navigation/drive_state.py
+++ b/starter_project/autonomy/src/navigation/drive_state.py
@@ -10,21 +10,31 @@ def __init__(self, context: Context):
         super().__init__(
             context,
             # TODO:
-            add_outcomes=["TODO: add outcomes here"],
+            add_outcomes=["driving_to_point", "reached_point"],
         )
 
     def evaluate(self, ud):
         target = np.array([5.5, 2.0, 0.0])
 
         # TODO: get the rover's pose, if it doesn't exist stay in DriveState (with outcome "driving_to_point")
-
+        SE3_pose = self.context.rover.get_pose()
+        if SE3_pose is None:
+            return "driving_to_point
+            "
         # TODO: get the drive command and completion status based on target and pose
         # (HINT: use get_drive_command(), with completion_thresh set to 0.7 and turn_in_place_thresh set to 0.2)
+        drive_command, completion_status = get_drive_command(
+            target_pos = target, rover_pose = SE3_pose, completion_thresh = 0.7, turn_in_place_thresh = 0.2
+        )
 
         # TODO: if we are finished getting to the target, go to TagSeekState (with outcome "reached_point")
+        if completion_status:
+            return "reached_point"
 
         # TODO: send the drive command to the rover
+        self.context.rover.send_drive_command(drive_command)
 
         # TODO: tell smach to stay in the DriveState by returning with outcome "driving_to_point"
+        return "driving_to_point"
 
         pass
diff --git a/starter_project/autonomy/src/navigation/navigation_starter_project.py b/starter_project/autonomy/src/navigation/navigation_starter_project.py
index a3f00f0dd..1bbedeed7 100755
--- a/starter_project/autonomy/src/navigation/navigation_starter_project.py
+++ b/starter_project/autonomy/src/navigation/navigation_starter_project.py
@@ -30,6 +30,11 @@ def __init__(self, context: Context):
         self.sis.start()
         with self.state_machine:
             # TODO: add DriveState and its transitions here
+            self.state_machine.add(
+                "DriveState",
+                DriveState(self.context),
+                transitions={"driving_to_point": "DriveState", "reached_point": "TagSeekState"},
+            )
 
             # DoneState and its transitions
             self.state_machine.add(
@@ -38,6 +43,12 @@ def __init__(self, context: Context):
                 transitions={"done": "DoneState"},
             )
             # TODO: add TagSeekState and its transitions here
+            self.state_machine.add(
+                "TagSeekState",
+                TagSeekState(self.context),
+                transitions={"failure": "DoneState", "working": "TagSeekState", "success": "DoneState"},
+            )
+
 
     def run(self):
         self.state_machine.execute()
@@ -53,6 +64,7 @@ def stop(self):
 
 def main():
     # TODO: init a node called "navigation"
+    rospy.init_node("navigation")
 
     # context and navigation objects
     context = Context()
diff --git a/starter_project/autonomy/src/navigation/tag_seek.py b/starter_project/autonomy/src/navigation/tag_seek.py
index 577731516..1254e1935 100644
--- a/starter_project/autonomy/src/navigation/tag_seek.py
+++ b/starter_project/autonomy/src/navigation/tag_seek.py
@@ -9,22 +9,34 @@ def __init__(self, context: Context):
         super().__init__(
             context,
             # TODO: add outcomes
-            add_outcomes=["TODO: add outcomes here"],
+            add_outcomes=["failure", "working", "success"],
         )
 
     def evaluate(self, ud):
         DISTANCE_TOLERANCE = 0.99
         ANUGLAR_TOLERANCE = 0.3
         # TODO: get the tag's location and properties (HINT: use get_fid_data() from context.env)
+        tag = self.context.env.get_fid_data()
 
         # TODO: if we don't have a tag: go to the DoneState (with outcome "failure")
+        if tag is None:
+            return "failure"
 
         # TODO: if we are within angular and distance tolerances: go to DoneState (with outcome "success")
+        if tag.closenessMetric < DISTANCE_TOLERANCE and abs(tag.xTagCenterPixel) < ANUGLAR_TOLERANCE:
+            return "success"
 
         # TODO: figure out the Twist command to be applied to move the rover closer to the tag
+        twist = Twist()
+        if tag.closenessMetric >= DISTANCE_TOLERANCE:
+            twist.linear.x = 1
+        if abs(tag.closenessMetric) >= ANUGLAR_TOLERANCE:
+            twist.angular.z = tag.xTagCenterPixel
 
         # TODO: send Twist command to rover
+        self.context.rover.send_drive_command(twist)
 
         # TODO: stay in the TagSeekState (with outcome "working")
+        return "working"
 
         pass
diff --git a/starter_project/autonomy/src/perception.cpp b/starter_project/autonomy/src/perception.cpp
index a20191410..658781a05 100644
--- a/starter_project/autonomy/src/perception.cpp
+++ b/starter_project/autonomy/src/perception.cpp
@@ -26,7 +26,7 @@ namespace mrover {
         // Create a publisher for our tag topic
         // See: http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29
         // TODO: uncomment me!
-        // mTagPublisher = mNodeHandle.advertise<StarterProjectTag>("tag", 1);
+        mTagPublisher = mNodeHandle.advertise<StarterProjectTag>("tag", 1);
 
         mTagDetectorParams = cv::makePtr<cv::aruco::DetectorParameters>();
         mTagDictionary = cv::makePtr<cv::aruco::Dictionary>(cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50));
@@ -54,15 +54,49 @@ namespace mrover {
         tags.clear(); // Clear old tags in output vector
 
         // TODO: implement me!
+        cv::aruco::detectMarkers(image, mTagDictionary, mTagCorners, mTagIds, mTagDetectorParams);
+
+        for (size_t i = 0; i < mTagIds.size(); ++i) {
+            StarterProjectTag tag;
+            tag.tagId = mTagIds[i];
+            std::pair<float, float> center = getCenterFromTagCorners(mTagCorners[i]);
+
+            tag.xTagCenterPixel = (center.first - image.cols/2) / image.cols;
+            tag.yTagCenterPixel = (center.second - image.rows/2) / image.rows;
+            tag.closenessMetric = getClosenessMetricFromTagCorners(image, mTagCorners[i]);
+            mTags.push_back(tag);
+        }
+        
     }
 
     auto Perception::selectTag(cv::Mat const& image, std::vector<StarterProjectTag> const& tags) -> StarterProjectTag { // NOLINT(*-convert-member-functions-to-static)
         // TODO: implement me!
-        return {};
+        if (tags.empty()) {
+            StarterProjectTag tag;
+            tag.tagId = -1;
+            return tag;
+        } else if (tags.size() == 1) { 
+            return tags[0];
+        } else {
+            int minTagIndex = 0;
+            double minTagDist = sqrt(pow(tags[0].xTagCenterPixel, 2) + pow(tags[0].yTagCenterPixel,2));
+
+            for (size_t i = 1; i < tags.size(); ++i) {
+                double currentTagDist = sqrt(pow(tags[i].xTagCenterPixel, 2) + pow(tags[i].yTagCenterPixel,2));
+
+                if (currentTagDist < minTagDist) {
+                    minTagDist = currentTagDist;
+                    minTagIndex = i;
+                }
+            }
+            
+            return tags[minTagIndex];
+        } 
     }
 
     auto Perception::publishTag(StarterProjectTag const& tag) -> void {
         // TODO: implement me!
+        mTagPublisher.publish(tag);
     }
 
     auto Perception::getClosenessMetricFromTagCorners(cv::Mat const& image, std::vector<cv::Point2f> const& tagCorners) -> float { // NOLINT(*-convert-member-functions-to-static)
@@ -71,12 +105,34 @@ namespace mrover {
         // hint: try not overthink, this metric does not have to be perfectly accurate, just correlated to distance away from a tag
 
         // TODO: implement me!
-        return {};
+        float areaImage = image.rows * image.cols;
+        cv::Point2f topLeft = tagCorners[0];
+        cv::Point2f topRight = tagCorners[1];
+        cv::Point2f bottomLeft = tagCorners[2];
+
+        float tagWidth = topRight.x - topLeft.x;
+        float tagHeight = bottomLeft.y - topLeft.y;
+        ROS_INFO("tag width: %f tag height: %f", tagWidth, tagHeight);
+
+        float ratio = 1 - (abs(tagWidth * tagHeight) / areaImage);
+
+        return ratio;
     }
 
     auto Perception::getCenterFromTagCorners(std::vector<cv::Point2f> const& tagCorners) -> std::pair<float, float> { // NOLINT(*-convert-member-functions-to-static)
         // TODO: implement me!
-        return {};
+        int xSum = 0;
+        int ySum = 0;
+
+        for (cv::Point2f corner : tagCorners) {
+            xSum += corner.x;
+            ySum += corner.y;
+        }
+
+        std::pair<float, float> center;
+        center.first = xSum;
+        center.second = ySum;
+        return center;
     }
 
 } // namespace mrover