Index: trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp	(revision 170)
+++ trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp	(revision 173)
@@ -29,4 +29,5 @@
 #include <Vector3D.h>
 #include <RotationMatrix.h>
+#include <OneAxisRotation.h>
 
 #include <math.h>
@@ -135,5 +136,8 @@
       filteredAngRates.z = Dequeue(&buf);
 
-      ahrsData->SetQuaternionAndAngularRates(eulerAngles.ToQuaternion(),
+      Quaternion quaternion=eulerAngles.ToQuaternion();
+      self->ApplyRotation(filteredAngRates);
+      self->ApplyRotation(quaternion);
+      ahrsData->SetQuaternionAndAngularRates(quaternion,
                                              filteredAngRates);
     } else if (command ==
@@ -164,6 +168,11 @@
       matrix(2, 2) = Dequeue(&buf);
 
-      ahrsData->SetQuaternionAndAngularRates(matrix.ToQuaternion(),
+      Quaternion quaternion=matrix.ToQuaternion();
+      self->ApplyRotation(filteredAngRates);
+      self->ApplyRotation(quaternion);
+      ahrsData->SetQuaternionAndAngularRates(quaternion,
                                              filteredAngRates);
+                                             
+      self->ApplyRotation(rawAcc);
       imuData->SetRawAcc(rawAcc);
     } else if (command == Gx3_25_imu::Quaternion) {
@@ -183,4 +192,6 @@
       filteredAngRates.z = 0;
 
+      self->ApplyRotation(filteredAngRates);
+      self->ApplyRotation(quaternion);
       ahrsData->SetQuaternionAndAngularRates(quaternion,
                                              filteredAngRates);
@@ -207,5 +218,4 @@
     imuData->SetDataTime(imuTime);
     ahrsData->SetDataTime(imuTime);
-    self->UpdateImu();
     self->ProcessUpdate(ahrsData);
   }
Index: trunk/lib/FlairSensorActuator/src/Imu.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/Imu.cpp	(revision 170)
+++ trunk/lib/FlairSensorActuator/src/Imu.cpp	(revision 173)
@@ -64,15 +64,18 @@
 }
 
-void Imu::UpdateImu() {
+void Imu::ApplyRotation(Vector3Df& vector) {
   if (rotation == NULL) {
     Err("not applicable for simulation part.\n");
     return;
   }
-  Vector3Df rawAcc, rawMag, rawGyr;
-  imuData->GetRawAccMagAndGyr(rawAcc, rawMag, rawGyr);
-  rotation->ComputeRotation(rawAcc);
-  rotation->ComputeRotation(rawGyr);
-  rotation->ComputeRotation(rawMag);
-  imuData->SetRawAccMagAndGyr(rawAcc, rawMag, rawGyr);
+  rotation->ComputeRotation(vector);
+}
+
+void Imu::ApplyRotation(Quaternion& quaternion) {
+  if (rotation == NULL) {
+    Err("not applicable for simulation part.\n");
+    return;
+  }
+  rotation->ComputeRotation(quaternion);
 }
 
Index: trunk/lib/FlairSensorActuator/src/Imu.h
===================================================================
--- trunk/lib/FlairSensorActuator/src/Imu.h	(revision 170)
+++ trunk/lib/FlairSensorActuator/src/Imu.h	(revision 173)
@@ -15,4 +15,5 @@
 
 #include <IODevice.h>
+#include <Vector3D.h>
 
 namespace flair {
@@ -121,12 +122,22 @@
 
   /*!
-  * \brief UpdateImu
+  * \brief ApplyRotation
   *
-  * The reimplemented class must call this function as soon as IMU datas are
-  *available. \n
+  * The reimplemented class must call this function to rotate IMU datas, before filling the ImuData. \n
   * It handles the data rotation if it was defined.
   *
+  * \param vector vector to apply rotation to
   */
-  void UpdateImu();
+  void ApplyRotation(core::Vector3Df& vector);
+  
+  /*!
+  * \brief ApplyRotation
+  *
+  * The reimplemented class must call this function to rotate IMU datas, before filling the ImuData. \n
+  * It handles the data rotation if it was defined.
+  *
+  * \param quaternion quaternion to apply rotation to
+  */
+  void ApplyRotation(core::Quaternion& quaternion);
 
   /*!
Index: trunk/lib/FlairSensorActuator/src/SimuImu.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/SimuImu.cpp	(revision 170)
+++ trunk/lib/FlairSensorActuator/src/SimuImu.cpp	(revision 173)
@@ -105,12 +105,14 @@
 
     shmem->Read((char *)&state, sizeof(imu_states_t));
-    ahrsData->SetQuaternionAndAngularRates(
-        Quaternion(state.q0, state.q1, state.q2, state.q3),
-        Vector3Df(state.wx, state.wy, state.wz));
+    
+    Quaternion  quaternion(state.q0, state.q1, state.q2, state.q3);
+    Vector3Df angRate(state.wx, state.wy, state.wz);
+    ApplyRotation(angRate);
+    ApplyRotation(quaternion);
+    ahrsData->SetQuaternionAndAngularRates(quaternion,angRate);
 
     imuData->SetDataTime(GetTime());
     ahrsData->SetDataTime(GetTime());
 
-    UpdateImu();
     ProcessUpdate(ahrsData);
   }
