diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
index 642f2ca1285b1a4a9c5c9548991bc9806859a377..0499d45ff74207fcfec955dddaf7c22382b7525a 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
+++ b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
@@ -19,23 +19,26 @@ void InertialMeasurementUnitObserver::onInitObserver()
 {
     usingTopic(getProperty<std::string>("IMUTopicName").getValue());
 
-
     offerConditionCheck("updated", new ConditionCheckUpdated());
     offerConditionCheck("larger", new ConditionCheckLarger());
     offerConditionCheck("equals", new ConditionCheckEquals());
     offerConditionCheck("smaller", new ConditionCheckSmaller());
+
+    offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
 }
 
 
 
 void InertialMeasurementUnitObserver::onConnectObserver()
 {
+    debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
 }
 
 
 void InertialMeasurementUnitObserver::onExitObserver()
 {
-
+    debugDrawerPrx->removePoseVisu("IMU", "orientation");
+    debugDrawerPrx->removeLineVisu("IMU", "acceleration");
 }
 
 void InertialMeasurementUnitObserver::reportSensorValues(const std::string &device, const std::string &name, const IMUData &values, const TimestampBasePtr &timestamp, const Ice::Current &c)
@@ -44,10 +47,10 @@ void InertialMeasurementUnitObserver::reportSensorValues(const std::string &devi
 
     TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
 
-    Vector3BasePtr acceleration = new Vector3(values.acceleration.at(0), values.acceleration.at(1), values.acceleration.at(2));
-    Vector3BasePtr gyroscopeRotation = new Vector3(values.gyroscopeRotation.at(0), values.gyroscopeRotation.at(1), values.gyroscopeRotation.at(2));
-    Vector3BasePtr magneticRotation = new Vector3(values.magneticRotation.at(0), values.magneticRotation.at(1), values.magneticRotation.at(2));
-    QuaternionBasePtr orientationQuaternion =  new Quaternion(values.orientationQuaternion.at(0), values.orientationQuaternion.at(1), values.orientationQuaternion.at(2), values.orientationQuaternion.at(3));
+    Vector3Ptr acceleration = new Vector3(values.acceleration.at(0), values.acceleration.at(1), values.acceleration.at(2));
+    Vector3Ptr gyroscopeRotation = new Vector3(values.gyroscopeRotation.at(0), values.gyroscopeRotation.at(1), values.gyroscopeRotation.at(2));
+    Vector3Ptr magneticRotation = new Vector3(values.magneticRotation.at(0), values.magneticRotation.at(1), values.magneticRotation.at(2));
+    QuaternionPtr orientationQuaternion =  new Quaternion(values.orientationQuaternion.at(0), values.orientationQuaternion.at(1), values.orientationQuaternion.at(2), values.orientationQuaternion.at(3));
 
     if(!existsChannel(device))
     {
@@ -70,6 +73,21 @@ void InertialMeasurementUnitObserver::reportSensorValues(const std::string &devi
         setDataField(device, "timestamp", timestampPtr);
     }
     updateChannel(device);
+
+    Eigen::Vector3f zero;
+    zero.setZero();
+
+    DrawColor color;
+    color.r = 255;
+    color.g = 255;
+
+    Eigen::Vector3f ac = acceleration->toEigen();
+    ac *= 10;
+
+    debugDrawerPrx->setLineVisu("IMU", "acceleration", new Vector3(), new Vector3(ac), 2.0f, color);
+
+    PosePtr posePtr = new Pose(orientationQuaternion->toEigen(), zero);
+    debugDrawerPrx->setPoseVisu("IMU", "orientation", posePtr);
 }
 
 
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
index 6ce41f2bf336cb98ca4288f77887e021d6ec0315..2c31902f36b7470c1ddaa797556b6b1e822921f5 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
+++ b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
@@ -26,6 +26,7 @@
 
 #include <RobotAPI/interface/units/InertialMeasurementUnit.h>
 #include <Core/observers/Observer.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
 
 namespace armarx
@@ -39,6 +40,7 @@ namespace armarx
             ComponentPropertyDefinitions(prefix)
         {
             defineOptionalProperty<std::string>("IMUTopicName", "IMUValues", "Name of the IMU Topic.");
+            defineOptionalProperty<std::string>("DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
         }
     };
 
@@ -65,6 +67,7 @@ namespace armarx
 
       private:
          Mutex dataMutex;
+         DebugDrawerInterfacePrx debugDrawerPrx;
 
 
     };
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.cpp b/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.cpp
index 65116cadf08f69d76d71a00370981f6ac6a37969..2857dca6918e593c1df02fa28bf5e38630d90d75 100644
--- a/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.cpp
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.cpp
@@ -110,10 +110,10 @@ void XsensIMU::onInitIMU()
     SetDispatchingMode(IMU::IIMUEventDispatcher::eDecoupled);
     SetMaximalPendingEvents(5);
 
-    IMUDevice.SetFusion(IMU::CIMUDevice::eMeanFusion,16);
+    IMUDevice.SetFusion(IMU::CIMUDevice::eMeanFusion, 4);
     IMUDevice.RegisterEventDispatcher(this);
 
-    IMUDevice.Connect(_IMU_DEVICE_DEFAUL_CONNECTION_, IMU::CIMUDevice::eSamplingFrequency_512HZ);
+    IMUDevice.Connect(_IMU_DEVICE_DEFAUL_CONNECTION_, IMU::CIMUDevice::eSamplingFrequency_120HZ);
 }
 
 void XsensIMU::onStartIMU()