diff --git a/config/bno055_imu_sim_config.yaml b/config/bno055_imu_sim_config.yaml
index bb1391e73da055cbe633c16c4b05db0b75c29f8b..997560290c3969171a5e2a9b50eaa22e86f3a2f4 100644
--- a/config/bno055_imu_sim_config.yaml
+++ b/config/bno055_imu_sim_config.yaml
@@ -1,5 +1,6 @@
 rate: 100.0
-namespace: '/'
+namespace: '/imu'
+tf_prefix: '/bno055'
 imu_topic: 'imu'
 accel_offset_x: 0.08
 accel_offset_y: 0.08
diff --git a/config/microstrain_3dm_gx3_imu_sim_config.yaml b/config/microstrain_3dm_gx3_imu_sim_config.yaml
index 6e81b87843cdca74916fe0559e2d579df88194b8..e7ea59231fec9fc11dff691a2a3779c5fdf6073a 100644
--- a/config/microstrain_3dm_gx3_imu_sim_config.yaml
+++ b/config/microstrain_3dm_gx3_imu_sim_config.yaml
@@ -1,5 +1,6 @@
 rate: 100.0
 namespace: '/'
+tf_prefix: '/'
 imu_topic: 'imu'
 accel_offset_x: 0.002
 accel_offset_y: 0.002
diff --git a/launch/bno055_imu_sim.launch b/launch/bno055_imu_sim.launch
index 679a8785dab4b3fe68b9b17fe6b6287d44d1ca37..1b1c67231a906cf4cff5d4f5d75248497535ba5f 100644
--- a/launch/bno055_imu_sim.launch
+++ b/launch/bno055_imu_sim.launch
@@ -18,15 +18,17 @@
     <arg name="pub_clock_frequency" value="100"/>
   </include>
 
-  <param name="robot_description"
-         command="$(find xacro)/xacro --inorder '$(find iri_bno055_imu_description)/urdf/bno055_imu_example.xacro'" />
+  <group ns="imu">
+    <param name="robot_description"
+           command="$(find xacro)/xacro --inorder '$(find iri_bno055_imu_description)/urdf/bno055_imu_example.xacro'" />
 
-  <node pkg="robot_state_publisher" type="state_publisher" name="bno055_tf_broadcaster">
-    <param name="tf_prefix" type="string" value="/bno055"/>
-    <param name="publish_frequency" type="double" value="20.0"/>
-  </node>
-
-  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model bno055_imu -x 0 -y 0 -z 0 -R 0 -P 0 -Y 0" />
+    <node pkg="robot_state_publisher" type="state_publisher" name="bno055_tf_broadcaster">
+      <param name="tf_prefix" type="string" value="/bno055"/>
+      <param name="publish_frequency" type="double" value="20.0"/>
+    </node>
+  
+    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model imu -x 0 -y 0 -z 0 -R 0 -P 0 -Y 0" />
+  </group>
 
 </launch>
 
diff --git a/urdf/imu.gazebo b/urdf/imu.gazebo
index 71d2a5235d3aeec6f932ed6a7e4b5d33aa13127b..2b3d85bfb1cd9bee96a88cd9ee686e54c9192b08 100644
--- a/urdf/imu.gazebo
+++ b/urdf/imu.gazebo
@@ -12,6 +12,7 @@
         <updateRate>${properties['rate']}</updateRate>
         <robotNamespace>${properties['namespace']}</robotNamespace>
         <bodyName>${name}</bodyName>
+        <frameId>${properties['tf_prefix']}/${name}</frameId>
         <topicName>${properties['imu_topic']}</topicName>
         <accelOffset>${properties['accel_offset_x']} ${properties['accel_offset_y']} ${properties['accel_offset_z']}</accelOffset>
         <accelDrift>${properties['accel_drift_x']} ${properties['accel_drift_y']} ${properties['accel_drift_z']}</accelDrift>
@@ -19,9 +20,9 @@
         <rateOffset>${properties['rate_offset_x']} ${properties['rate_offset_y']} ${properties['rate_offset_z']}</rateOffset>
         <rateDrift>${properties['rate_drift_x']} ${properties['rate_drift_y']} ${properties['rate_drift_z']}</rateDrift>
         <rateGaussianNoise>${properties['rate_noise_x']} ${properties['rate_noise_y']} ${properties['rate_noise_z']}</rateGaussianNoise>
-        <headingOffset>${properties['heading_offset']}</headingOffset>
-        <headingDrift>${properties['heading_drift']}</headingDrift>
-        <headingGaussianNoise>${properties['heading_noise']}</headingGaussianNoise>
+        <yawOffset>${properties['heading_offset']}</yawOffset>
+        <yawDrift>${properties['heading_drift']}</yawDrift>
+        <yawGaussianNoise>${properties['heading_noise']}</yawGaussianNoise>
       </plugin>
     </gazebo>
   </xacro:macro>