diff --git a/config/helena_bno055.yaml b/config/helena_bno055.yaml
index d8cd7ddea9d4ed31def95622bd57573778f4a0d8..a6dcc71713ecafc478af5448320c699d46c4506f 100644
--- a/config/helena_bno055.yaml
+++ b/config/helena_bno055.yaml
@@ -1,4 +1,78 @@
-update_rate: 1
+update_rate: 100
 serial_device: '/dev/ttyUSB1'
-tf_prefix: '/'
-frame_id: 'imu'
+tf_prefix: 'helena/'
+frame_id: 'imu_bno055'
+# 1 -> "Only enable the accelerometer"
+# 2 -> "Only enable the magnetometer"
+# 3 -> "Only enable the gyroscope"
+# 4 -> "Enable both accelerometer and magnetometer"
+# 5 -> "Enable both accelerometer and gyroscope"
+# 6 -> "Enable both magnetometer and gyroscope"
+# 7 -> "Enable all sensors"
+# 8 -> "IMU"
+# 9 -> "Compass"
+# 10 -> "M4G"
+# 11 -> "ndof off"
+# 12 -> "ndof"
+mode: 12
+# 0 -> "+/- 2G"
+# 1 -> "+/- 4G"
+# 2 -> "+/- 8G"
+# 3 -> "+/- 16G"
+accel_range: 1
+# 0 -> "7.81Hz"
+# 4 -> "15.63Hz"
+# 8 -> "32.25H"
+# 12 -> "62.5Hz"
+# 16 -> "125Hz"
+# 20 -> "250Hz"
+# 24 -> "500Hz"
+# 28 -> "1000Hz"
+accel_bandwidth: 12
+# 0 -> "Normal"
+# 32 -> "Suspend"
+# 64 -> "Low power 1"
+# 96 -> "Stand by"
+# 128 -> "Low power 2"
+# 160 -> "Seep suspend"
+accel_pwr_mode: 0
+# 0 -> "+/- 2000 dps"
+# 1 -> "+/- 1000 dps"
+# 2 -> "+/- 500 dps"
+# 3 -> "+/- 250 dps"
+# 4 -> "+/- 125 dps"
+gyro_range: 0
+# 0 -> "523Hz"
+# 8 -> "230Hz"
+# 16 -> "116Hz"
+# 24 -> "47Hz"
+# 32 -> "23Hz"
+# 40 -> "12Hz"
+# 48 -> "64Hz"
+# 56 -> "32Hz"
+gyro_bandwidth: 56
+# 0 -> "Normal"
+# 1 -> "Fast power up"
+# 2 -> "Deep suspend"
+# 3 -> "Suspend"
+# 4 -> "Advanced power save"
+gyro_pwr_mode: 0
+# 0 -> "2 Hz"
+# 1 -> "6 Hz"
+# 2 -> "8 Hz"
+# 3 -> "10 Hz"
+# 4 -> "15 Hz"
+# 5 -> "20 Hz"
+# 6 -> "25 Hz"
+# 7 -> "30 Hz"
+mag_rate: 3
+# 0 -> "Low power"
+# 8 -> "Regular"
+# 16 -> "Enhanced regular"
+# 24 -> "High accuracy"
+mag_op_mode: 1
+# 0 -> "Normal"
+# 32 -> "Sleep"
+# 64 -> "Suspend"
+# 96 -> "Force mode"
+mag_pwr_mode: 0
diff --git a/rviz/helena.rviz b/rviz/helena.rviz
index 26f30d750ac4aa9071eb51344108d48bf8c6fc11..1ca690148bed0bad9f5b5df0acc0ceaa66d1de15 100644
--- a/rviz/helena.rviz
+++ b/rviz/helena.rviz
@@ -449,7 +449,7 @@ Visualization Manager:
           Max Color: 255; 255; 255
           Max Intensity: 255
           Min Color: 0; 0; 0
-          Min Intensity: 0.5161057710647583
+          Min Intensity: 0.3743307888507843
           Name: robosense
           Position Transformer: XYZ
           Queue Size: 10
@@ -504,6 +504,15 @@ Visualization Manager:
           Transport Hint: raw
           Unreliable: false
           Value: true
+        - Alpha: 1
+          Class: rviz_plugin_tutorials/Imu
+          Color: 204; 51; 204
+          Enabled: true
+          History Length: 1
+          Name: Imu
+          Topic: /helena/sensors/bno055_imu/imu
+          Unreliable: false
+          Value: true
       Enabled: true
       Name: Sensors
   Enabled: true
@@ -534,25 +543,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 8.066530227661133
+      Distance: 6.229821681976318
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.1004096046090126
-        Y: 0.5926481485366821
-        Z: 1.295820713043213
+        X: -0.24123165011405945
+        Y: 0.42437949776649475
+        Z: 1.1607776880264282
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.7503980398178101
+      Pitch: 1.0303977727890015
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.3653981685638428
+      Yaw: 3.2804007530212402
     Saved: ~
 Window Geometry:
   Displays:
@@ -560,7 +569,7 @@ Window Geometry:
   Height: 846
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002f4fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000022d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e005200650061006c00530065006e0073006500430061006d0065007200610100000270000000c10000001600ffffff000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b0000004f300fffffffb0000000800540069006d0065010000000000000450000000000000000000000340000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002f4fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000022d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e005200650061006c00530065006e0073006500430061006d0065007200610100000270000000c10000001600ffffff000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b0000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000340000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   RealSenseCamera:
     collapsed: false
   Selection: