diff --git a/rviz/uw_lidar.rviz b/rviz/uw_lidar.rviz
index c9463cb..eaaeedb 100644
--- a/rviz/uw_lidar.rviz
+++ b/rviz/uw_lidar.rviz
@@ -6,8 +6,9 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
+        - /PointCloud21
       Splitter Ratio: 0.5
-    Tree Height: 676
+    Tree Height: 555
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -77,7 +78,7 @@ Visualization Manager:
       Size (Pixels): 3
       Size (m): 0.009999999776482582
       Style: Flat Squares
-      Topic: /nps_uwl/points
+      Topic: /nps_uwl_points
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
@@ -86,7 +87,7 @@ Visualization Manager:
   Global Options:
     Background Color: 48; 48; 48
     Default Light: true
-    Fixed Frame: nps_uwl/pulse_lidar_link
+    Fixed Frame: world
     Frame Rate: 30
   Name: root
   Tools:
@@ -110,33 +111,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 10.464838027954102
+      Distance: 24.759632110595703
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 4.820145606994629
-        Y: -1.0567362308502197
-        Z: -1.1478139162063599
+        X: 0
+        Y: 0
+        Z: 0
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.15979783236980438
+      Pitch: 0.40039798617362976
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.960989236831665
+      Yaw: 1.6653974056243896
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: true
-  Height: 561
-  Hide Left Dock: true
-  Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000001560000032dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b0000032d000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000224fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000224000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a70000003efc0100000002fb0000000800540069006d00650100000000000003a70000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000003a70000019700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -144,7 +145,7 @@ Window Geometry:
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: true
-  Width: 935
-  X: 2926
-  Y: 324
+    collapsed: false
+  Width: 1200
+  X: 3066
+  Y: 197
diff --git a/urdf/uw_lidar.xacro b/urdf/uw_lidar.xacro
index b752197..db2d468 100644
--- a/urdf/uw_lidar.xacro
+++ b/urdf/uw_lidar.xacro
@@ -33,11 +33,12 @@
 
   <!-- Generates an Underwater Lidar model and attaches it to a parent link 
        Sensor parameters align with those in the 3D at Depth SL3 datasheet -->
-  <xacro:macro name="uwl_macro" params="namespace parent_link *origin pan tilt">
+  <xacro:macro name="uwl_macro" params="namespace parent_link *origin pan tilt inertial_reference_frame">
     <xacro:uw_lidar_plugin_macro
       namespace="${namespace}"
       suffix=""
       parent_link="${parent_link}" 
+      reference_frame="${inertial_reference_frame}"
       pan="${pan}" 
       tilt="${tilt}" >
       <xacro:insert_block name="origin"/>
@@ -46,7 +47,7 @@
 
 
   <!-- Generates a Lidar sensor -->
-  <xacro:macro name="uw_lidar_sensor" params="namespace suffix update_rate">
+  <xacro:macro name="uw_lidar_sensor" params="namespace suffix update_rate reference_frame">
     <link name="${namespace}/uwl${suffix}_sensor_link">
       <inertial>
         <mass value="0.001"/>
@@ -70,8 +71,8 @@
        <always_on>true</always_on>
 
        <plugin name="pulse_lidar_sensing" filename="libgazebo_ros_velodyne_gpu_laser.so">
-           <topicName>nps_uwl/points</topicName>
-           <frameName>nps_uwl/pulse_lidar_link</frameName>
+           <topicName>nps_uwl_points</topicName>
+           <static_reference_frame>${reference_frame}</static_reference_frame>
            <min_intensity>0</min_intensity>
            <min_range>1</min_range>
            <max_range>20</max_range>
@@ -127,7 +128,7 @@
 
   <!-- Generates the uw_lidar model with stand and motion control plugin -->
   <xacro:macro name="uw_lidar_plugin_macro"
-    params="namespace suffix parent_link *origin pan tilt">
+    params="namespace suffix parent_link *origin pan tilt reference_frame">
 
     <link name="${namespace}/uwl${suffix}_link">
       <inertial>
@@ -240,7 +241,7 @@
 
     <xacro:uw_lidar_sensor
       namespace="${namespace}" suffix=""
-      update_rate="40">
+      update_rate="40" reference_frame="${reference_frame}">
     </xacro:uw_lidar_sensor>
 
     <gazebo>
diff --git a/urdf/uw_lidar_pedestal_robot.xacro b/urdf/uw_lidar_pedestal_robot.xacro
index 1984ee2..09aa5c7 100644
--- a/urdf/uw_lidar_pedestal_robot.xacro
+++ b/urdf/uw_lidar_pedestal_robot.xacro
@@ -1,6 +1,7 @@
 <?xml version="1.0"?>
 
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="uw_lidar_pedestal">
+  <xacro:arg name="inertial_reference_frame" default="robot_base_link"/>
 
   <xacro:include filename="$(find nps_uw_sensors_gazebo)/urdf/uw_lidar.xacro"/>
   <!-- Used for fixing robot to Gazebo 'base_link' -->
@@ -27,7 +28,9 @@
     </collision>
   </link>
 
-  <xacro:uwl_macro namespace="uwl" parent_link="robot_base_link" pan="-${pi/2}" tilt="0">
+  <xacro:uwl_macro namespace="uwl" parent_link="robot_base_link" 
+                   inertial_reference_frame="$(arg inertial_reference_frame)"
+                   pan="-${pi/2}" tilt="0">
     <origin xyz="0 0 0.25" rpy="0 0 -${pi/2}"/>
   </xacro:uwl_macro>
 
diff --git a/urdf/uw_lidar_robot.xacro b/urdf/uw_lidar_robot.xacro
index c59daf1..935815e 100644
--- a/urdf/uw_lidar_robot.xacro
+++ b/urdf/uw_lidar_robot.xacro
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="uw_lidar">
-
+  <xacro:arg name="inertial_reference_frame" default="world"/>
   <xacro:include filename="$(find nps_uw_sensors_gazebo)/urdf/uw_lidar.xacro"/>
 
   <link name="robot_base_link"/>
@@ -35,7 +35,9 @@
     <child link="robot_link"/>
   </joint>
 
-  <xacro:uwl_macro namespace="uwl" parent_link="robot_base_link" pan="0" tilt="0">
+  <xacro:uwl_macro namespace="uwl" parent_link="robot_base_link" 
+                   inertial_reference_frame="$(arg inertial_reference_frame)"
+                   pan="0" tilt="0">
     <origin xyz="0.48 0 1.41" rpy="0 0 ${pi/2}"/>
   </xacro:uwl_macro>