diff --git a/src/rove_bringup/config/VLP16_velodyne_params.yaml b/src/rove_bringup/config/VLP16_velodyne_params.yaml
index b18fc47..ed9a54f 100644
--- a/src/rove_bringup/config/VLP16_velodyne_params.yaml
+++ b/src/rove_bringup/config/VLP16_velodyne_params.yaml
@@ -8,7 +8,7 @@ velodyne_driver_node:
         read_once: false
         read_fast: false
         repeat_delay: 0.0
-        frame_id: velodyne
+        frame_id: velodyne_laser
         model: VLP16
         rpm: 600.0
         port: 2368
@@ -25,6 +25,6 @@ velodyne_transform_node:
         min_range: 0.9
         max_range: 130.0
         view_direction: 0.0
-        fixed_frame: ""
-        target_frame: ""
+        fixed_frame: "base_link"
+        target_frame: "velodyne_laser"
         organize_cloud: true
\ No newline at end of file
diff --git a/src/rove_bringup/config/vn_300.yaml b/src/rove_bringup/config/vn_300.yaml
index b6e02f0..ca892f6 100644
--- a/src/rove_bringup/config/vn_300.yaml
+++ b/src/rove_bringup/config/vn_300.yaml
@@ -94,7 +94,7 @@ vectornav:
       attitudeField: 0x0000          # ATTITUDEGROUP_NONE
       insField: 0x0000               # INSGROUP_NONE
       gps2Field: 0x0000              # GPSGROUP_NONE
-    frame_id: "vectornav"
+    frame_id: "sensor_link"
 
 vn_sensor_msgs:
   ros__parameters:
diff --git a/src/rove_bringup/launch/real.launch.py b/src/rove_bringup/launch/real.launch.py
index 4205eff..6b091e2 100644
--- a/src/rove_bringup/launch/real.launch.py
+++ b/src/rove_bringup/launch/real.launch.py
@@ -107,11 +107,24 @@ def create_controller_node(node_name:str):
     #     )
     # )
 
+    ###### Sensor ######
+    vectornav = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_rove_bringup, "launch", "vectornav.launch.py"),
+        ),
+    )
+
+    velodyne = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_rove_bringup, "launch", "velodyne.launch.py"),
+        ),
+    )
+
     return LaunchDescription([
             control_node,
             common,
             joint_state_broadcaster_spawner,
             *delayed_controller_nodes,
-            # vectornav,
-            # velodyne,
+            vectornav,
+            velodyne,
             ])
diff --git a/src/rove_description/config/basic.rviz b/src/rove_description/config/basic.rviz
index 6334030..d662f35 100644
--- a/src/rove_description/config/basic.rviz
+++ b/src/rove_description/config/basic.rviz
@@ -3,8 +3,7 @@ Panels:
     Help Height: 78
     Name: Displays
     Property Tree Widget:
-      Expanded:
-        - /RobotModel1/Status1
+      Expanded: ~
       Splitter Ratio: 0.5
     Tree Height: 517
   - Class: rviz_common/Selection
@@ -71,60 +70,37 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        rove_end_effector:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        rove_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rove_link_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rove_link_3:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rove_link_4:
+        sensor_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        rove_link_5:
+        track_fl:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        rove_link_6:
+        track_fr:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        rove_link_base:
+        track_l:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        sensor_link:
+        track_r:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        track_l:
+        track_rl:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        track_r:
+        track_rr:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
         velodyne_laser:
           Alpha: 1
           Show Axes: false
@@ -176,9 +152,9 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 0
+      Max Intensity: 125
       Min Color: 0; 0; 0
-      Min Intensity: 0
+      Min Intensity: 1
       Name: LaserScan
       Position Transformer: XYZ
       Selectable: true
@@ -202,40 +178,20 @@ Visualization Manager:
         All Enabled: false
         base_link:
           Value: true
-        flipper_fl:
-          Value: true
-        flipper_fr:
-          Value: true
-        flipper_rl:
-          Value: true
-        flipper_rr:
-          Value: true
-        map:
-          Value: true
-        odom:
-          Value: true
-        rove_end_effector:
-          Value: true
-        rove_link_1:
-          Value: true
-        rove_link_2:
-          Value: true
-        rove_link_3:
-          Value: true
-        rove_link_4:
-          Value: true
-        rove_link_5:
-          Value: true
-        rove_link_6:
+        sensor_link:
           Value: true
-        rove_link_base:
+        track_fl:
           Value: true
-        sensor_link:
+        track_fr:
           Value: true
         track_l:
           Value: true
         track_r:
           Value: true
+        track_rl:
+          Value: true
+        track_rr:
+          Value: true
         velodyne_laser:
           Value: true
         velodyne_link:
@@ -246,35 +202,22 @@ Visualization Manager:
       Show Axes: true
       Show Names: false
       Tree:
-        map:
-          odom:
-            base_link:
-              flipper_fl:
-                {}
-              flipper_fr:
-                {}
-              flipper_rl:
-                {}
-              flipper_rr:
-                {}
-              rove_link_base:
-                rove_link_1:
-                  rove_link_2:
-                    rove_link_3:
-                      rove_link_4:
-                        rove_link_5:
-                          rove_link_6:
-                            rove_end_effector:
-                              {}
-              sensor_link:
-                {}
-              track_l:
-                {}
-              track_r:
-                {}
-              velodyne_link:
-                velodyne_laser:
-                  {}
+        base_link:
+          sensor_link:
+            {}
+          track_l:
+            track_fl:
+              {}
+            track_rl:
+              {}
+          track_r:
+            track_fr:
+              {}
+            track_rr:
+              {}
+          velodyne_link:
+            velodyne_laser:
+              {}
       Update Interval: 0
       Value: true
     - Alpha: 0.699999988079071
@@ -382,7 +325,7 @@ Visualization Manager:
         y_scale: 1
         z_scale: 1
       Class: rviz_imu_plugin/Imu
-      Enabled: false
+      Enabled: true
       Name: Imu
       Topic:
         Depth: 5
@@ -391,12 +334,12 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /imu
-      Value: false
+      Value: true
       fixed_frame_orientation: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: map
+    Fixed Frame: base_link
     Frame Rate: 30
   Name: root
   Tools:
@@ -439,7 +382,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 15.358924865722656
+      Distance: 7.311348915100098
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -454,10 +397,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.900398313999176
+      Pitch: 1.0553982257843018
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 5.728583812713623
+      Yaw: 4.913582801818848
     Saved: ~
 Window Geometry:
   Displays:
diff --git a/src/rove_description/package.xml b/src/rove_description/package.xml
index 340d14a..596b190 100644
--- a/src/rove_description/package.xml
+++ b/src/rove_description/package.xml
@@ -10,6 +10,7 @@
   <exec_depend>rviz2</exec_depend>
   <exec_depend>xacro</exec_depend>
   <exec_depend>velodyne_description</exec_depend>
+  <exec_depend>rviz_imu_plugin</exec_depend>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
diff --git a/src/rove_description/urdf/command.urdf.xacro b/src/rove_description/urdf/command.urdf.xacro
deleted file mode 100644
index c265b82..0000000
--- a/src/rove_description/urdf/command.urdf.xacro
+++ /dev/null
@@ -1,101 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-    <plugin filename="gz-sim-triggered-publisher-system"
-        name="gz::sim::systems::TriggeredPublisher">
-        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
-            <match field="data">87</match>
-        </input>
-        <output type="gz.msgs.Twist" topic="cmd_vel">
-            linear: {x: 1.0}, angular: {z: 0.0}
-        </output>
-    </plugin>
-
-    <!-- Moving Backward: X -->
-    <plugin filename="gz-sim-triggered-publisher-system"
-        name="gz::sim::systems::TriggeredPublisher">
-        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
-            <match field="data">88</match>
-        </input>
-        <output type="gz.msgs.Twist" topic="cmd_vel">
-            linear: {x: -1.0}, angular: {z: 0.0}
-        </output>
-    </plugin>
-
-    <!-- Stop: S -->
-    <plugin filename="gz-sim-triggered-publisher-system"
-        name="gz::sim::systems::TriggeredPublisher">
-        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
-            <match field="data">83</match>
-        </input>
-        <output type="gz.msgs.Twist" topic="cmd_vel">
-            linear: {x: 0.0}, angular: {z: 0.0}
-        </output>
-    </plugin>
-
-    <!-- Left: A -->
-    <plugin filename="gz-sim-triggered-publisher-system"
-        name="gz::sim::systems::TriggeredPublisher">
-        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
-            <match field="data">65</match>
-        </input>
-        <output type="gz.msgs.Twist" topic="cmd_vel">
-            linear: {x: 0.0}, angular: {z: 1.0}
-        </output>
-    </plugin>
-
-    <!-- Right: D -->
-    <plugin filename="gz-sim-triggered-publisher-system"
-        name="gz::sim::systems::TriggeredPublisher">
-        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
-            <match field="data">68</match>
-        </input>
-        <output type="gz.msgs.Twist" topic="cmd_vel">
-            linear: {x: 0.0}, angular: {z: -1.0}
-        </output>
-    </plugin>
-
-    <!-- Forward Left: Q -->
-    <plugin filename="gz-sim-triggered-publisher-system"
-        name="gz::sim::systems::TriggeredPublisher">
-        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
-            <match field="data">81</match>
-        </input>
-        <output type="gz.msgs.Twist" topic="cmd_vel">
-            linear: {x: 1.0}, angular: {z: 1.0}
-        </output>
-    </plugin>
-
-    <!-- Forward Right: E -->
-    <plugin filename="gz-sim-triggered-publisher-system"
-        name="gz::sim::systems::TriggeredPublisher">
-        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
-            <match field="data">69</match>
-        </input>
-        <output type="gz.msgs.Twist" topic="cmd_vel">
-            linear: {x: 1.0}, angular: {z: -1.0}
-        </output>
-    </plugin>
-
-    <!-- Back Left: Z -->
-    <plugin filename="gz-sim-triggered-publisher-system"
-        name="gz::sim::systems::TriggeredPublisher">
-        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
-            <match field="data">90</match>
-        </input>
-        <output type="gz.msgs.Twist" topic="cmd_vel">
-            linear: {x: -1.0}, angular: {z: 1.0}
-        </output>
-    </plugin>
-
-    <!-- Back Right: C -->
-    <plugin filename="gz-sim-triggered-publisher-system"
-        name="gz::sim::systems::TriggeredPublisher">
-        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
-            <match field="data">67</match>
-        </input>
-        <output type="gz.msgs.Twist" topic="cmd_vel">
-            linear: {x: -1.0}, angular: {z: -1.0}
-        </output>
-    </plugin>
-
-</robot>
\ No newline at end of file
diff --git a/src/rove_description/urdf/fake_wheels.urdf.xacro b/src/rove_description/urdf/fake_wheels.urdf.xacro
deleted file mode 100644
index 21531e3..0000000
--- a/src/rove_description/urdf/fake_wheels.urdf.xacro
+++ /dev/null
@@ -1,53 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-  
-    <joint name="left_wheel_joint" type="fixed">
-      <parent link="base_link"/>
-      <child link="left_wheel_link"/>
-      <axis>
-        <xyz>0 0 1</xyz>
-        <limit>
-          <lower>-1.79769e+308</lower>
-          <upper>1.79769e+308</upper>
-        </limit>
-      </axis>
-  </joint>
-    
-  <link name="left_wheel_link">
-    <pose>0.554283 0.625029 -0.025 -1.5707 0 0</pose>
-    <inertial>
-      <mass>2</mass>
-      <inertia>
-        <ixx>0.145833</ixx>
-        <ixy>0</ixy>
-        <ixz>0</ixz>
-        <iyy>0.145833</iyy>
-        <iyz>0</iyz>
-        <izz>0.125</izz>
-      </inertia>
-    </inertial>
-    <visual name='visual'>
-      <geometry>
-        <sphere>
-          <radius>0.3</radius>
-        </sphere>
-      </geometry>
-      <material>
-        <ambient>0.2 0.2 0.2 1</ambient>
-        <diffuse>0.2 0.2 0.2 1</diffuse>
-        <specular>0.2 0.2 0.2 1</specular>
-      </material>
-    </visual>
-    <collision name='collision'>
-      <geometry>
-        <sphere>
-          <radius>0.3</radius>
-        </sphere>
-      </geometry>
-    </collision>
-</link>
-
-
-
-
-</robot>
diff --git a/src/rove_description/urdf/lidar.urdf.xacro b/src/rove_description/urdf/lidar.urdf.xacro
deleted file mode 100644
index 6d1c3df..0000000
--- a/src/rove_description/urdf/lidar.urdf.xacro
+++ /dev/null
@@ -1,14 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-
-    <xacro:property name="M_PI" value="3.1415926535897931" />
-    
-    <xacro:include filename="$(find rove_description)/urdf/velodyne.urdf.xacro" />
-
-    <xacro:velodyne_lidar name="velodyne" parent_link="base_link">
-
-        <origin xyz="0.0 0.0 0.4" rpy="0 0 0" />
-
-    </xacro:velodyne_lidar>
-
-</robot>
\ No newline at end of file
diff --git a/src/rove_description/urdf/rove.urdf.xacro b/src/rove_description/urdf/rove.urdf.xacro
index 9d1de3c..3dae78e 100644
--- a/src/rove_description/urdf/rove.urdf.xacro
+++ b/src/rove_description/urdf/rove.urdf.xacro
@@ -11,7 +11,6 @@
     </material>
 
     <xacro:include filename="$(find rove_description)/urdf/gazebo.urdf.xacro" />
-    <xacro:include filename="$(find rove_description)/urdf/lidar.urdf.xacro" />
     <xacro:include filename="$(find rove_description)/urdf/sensor.urdf.xacro" />
     
     <xacro:include filename="$(find rove_description)/urdf/flipper.urdf.xacro" />
@@ -20,8 +19,6 @@
     
     <!-- <xacro:include filename="$(find ovis_description)/urdf/ovis.urdf.xacro" /> -->
 
-
-
     <link name="base_link">
         <visual>
             <origin xyz="0 0 0" rpy="0 0 0"/>
diff --git a/src/rove_description/urdf/sensor.urdf.xacro b/src/rove_description/urdf/sensor.urdf.xacro
index 7f92b0b..4d17df4 100644
--- a/src/rove_description/urdf/sensor.urdf.xacro
+++ b/src/rove_description/urdf/sensor.urdf.xacro
@@ -1,5 +1,7 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:property name="M_PI" value="3.1415926535897931" />
+    <xacro:include filename="$(find rove_description)/urdf/velodyne.urdf.xacro" />
 
     <joint name="sensor_joint" type="fixed">
         <parent link="base_link"/>
@@ -27,6 +29,10 @@
         </collision>
     </link>
 
+    <xacro:velodyne_lidar name="velodyne" parent_link="base_link">
+        <origin xyz="0.0 0.0 0.4" rpy="0 0 0" />
+    </xacro:velodyne_lidar>
+
     <gazebo reference="sensor_link">
         <material>Gazebo/Black</material>
 
diff --git a/src/rove_slam/config/ekf.yaml b/src/rove_slam/config/ekf.yaml
index ddfefe3..261e3c6 100644
--- a/src/rove_slam/config/ekf.yaml
+++ b/src/rove_slam/config/ekf.yaml
@@ -21,7 +21,7 @@ ekf_filter_node_local:
                       false,  false,  true, # roll pitch yaw
                       false,  false,  false, # vx vy vz
                       false,  false,  true, # vroll vpitch vyaw
-                      true, true, false] # ax ay az
+                      false, false, false] # ax ay az
         imu0_queue_size: 20
 
 ekf_filter_node_global:
@@ -46,7 +46,7 @@ ekf_filter_node_global:
                       false,  false,  true, # roll pitch yaw
                       false,  false,  false, # vx vy vz
                       false,  false,  true, # vroll vpitch vyaw
-                      true, true, false] # ax ay az
+                      false, false, false] # ax ay az
         imu0_queue_size: 20
 
         # description from https://github.com/cra-ros-pkg/robot_localization/blob/ros2/params/ekf.yaml