diff --git a/sw/airborne/subsystems/ahrs/dcm/analogimu.c b/sw/airborne/subsystems/ahrs/dcm/analogimu.c
index 53d822bef3..528abf8f95 100644
--- a/sw/airborne/subsystems/ahrs/dcm/analogimu.c
+++ b/sw/airborne/subsystems/ahrs/dcm/analogimu.c
@@ -90,39 +90,23 @@ void analog_imu_downlink( void ) {
 
 void estimator_update_state_analog_imu( void ) {
 
-  /* Offset is set dynamic on Ground*/
-  /*Gyro_Vector[0]= -gyro_to_zero[G_ROLL]   + gyro[G_ROLL];
-  Gyro_Vector[1]= -gyro_to_zero[G_PITCH]  + gyro[G_PITCH];
-  Gyro_Vector[2]= -gyro_to_zero[G_PITCH]  + gyro[G_YAW];*/
-
   //FIXME run aligner to set gyro neutrals on ground
 
   /* converts gyro to floating point */
-  struct FloatRates gyro_float;
   RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
-  Gyro_Vector[0]= gyro_float.p;
-  Gyro_Vector[1]= gyro_float.q;
-  Gyro_Vector[2]= gyro_float.r;
-
-  struct FloatVect3 accel_float;
   ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
-  Accel_Vector[0] = accel_float.x;
-  Accel_Vector[1] = accel_float.y;
-  Accel_Vector[2] = accel_float.z;
-
 
   Matrix_update();
   Normalize();
 
+  // FIXME convert gps data here
 
   Drift_correction();
   Euler_angles();
 
-  // return euler angles to phi and theta
+  // export results to estimator
   estimator_phi = euler.phi-imu_roll_neutral;
-  //estimator_phi = angle[ANG_ROLL];
   estimator_theta = euler.theta-imu_pitch_neutral;
-  //estimator_theta = angle[ANG_PITCH];
   estimator_psi = euler.psi;
 
 }
diff --git a/sw/airborne/subsystems/ahrs/dcm/dcm.c b/sw/airborne/subsystems/ahrs/dcm/dcm.c
index d091d82d61..7b387e5e14 100644
--- a/sw/airborne/subsystems/ahrs/dcm/dcm.c
+++ b/sw/airborne/subsystems/ahrs/dcm/dcm.c
@@ -11,8 +11,8 @@
 // DCM Working variables
 float G_Dt=0.05;
 
-float Gyro_Vector[3] = {0,0,0};
-float Accel_Vector[3] = {0,0,0};
+struct FloatRates gyro_float = {0,0,0};
+struct FloatVect3 accel_float = {0,0,0};
 
 float Omega_Vector[3]= {0,0,0};     //Corrected Gyro_Vector data
 float Omega_P[3]= {0,0,0};		//Omega Proportional correction
@@ -207,7 +207,7 @@ void Drift_correction(void)
   //*****Roll and Pitch***************
 
   // Calculate the magnitude of the accelerometer vector
-  Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
+  Accel_magnitude = sqrt(accel_float.x*accel_float.x + accel_float.y*accel_float.y + accel_float.z*accel_float.z);
   Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
   // Dynamic weighting of accelerometer info (reliability filter)
   // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
@@ -222,7 +222,7 @@ void Drift_correction(void)
   }
   #endif
 
-  Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
+  Vector_Cross_Product(&errorRollPitch[0],&accel_float.x,&DCM_Matrix[2][0]); //adjust the ground of reference
   Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
 
   Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
@@ -270,13 +270,13 @@ void Drift_correction(void)
 
 void Matrix_update(void)
 {
-  Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);  //adding proportional term
+  Vector_Add(&Omega[0], &gyro_float.p, &Omega_I[0]);  //adding proportional term
   Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
 
   if (gps_mode==3)    //Remove centrifugal acceleration.
   {
-    Accel_Vector[1] += speed_3d*Omega[2];  // Centrifugal force on Acc_y = GPS_speed*GyroZ
-    Accel_Vector[2] -= speed_3d*Omega[1];  // Centrifugal force on Acc_z = GPS_speed*GyroY
+    accel_float.y += speed_3d*Omega[2];  // Centrifugal force on Acc_y = GPS_speed*GyroZ
+    accel_float.z -= speed_3d*Omega[1];  // Centrifugal force on Acc_z = GPS_speed*GyroY
   }
 
 
@@ -292,13 +292,13 @@ void Matrix_update(void)
   Update_Matrix[2][2]=0;
  #else                    // Uncorrected data (no drift correction)
   Update_Matrix[0][0]=0;
-  Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
-  Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
-  Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
+  Update_Matrix[0][1]=-G_Dt*gyro_float.r;//-z
+  Update_Matrix[0][2]=G_Dt*gyro_float.q;//y
+  Update_Matrix[1][0]=G_Dt*gyro_float.r;//z
   Update_Matrix[1][1]=0;
-  Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
-  Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
-  Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
+  Update_Matrix[1][2]=-G_Dt*gyro_float.p;
+  Update_Matrix[2][0]=-G_Dt*gyro_float.q;
+  Update_Matrix[2][1]=G_Dt*gyro_float.p;
   Update_Matrix[2][2]=0;
  #endif
 
diff --git a/sw/airborne/subsystems/ahrs/dcm/dcm.h b/sw/airborne/subsystems/ahrs/dcm/dcm.h
index 74f862ddda..6cd74ebb31 100644
--- a/sw/airborne/subsystems/ahrs/dcm/dcm.h
+++ b/sw/airborne/subsystems/ahrs/dcm/dcm.h
@@ -1,8 +1,8 @@
 #include "math/pprz_algebra_float.h"
 
 // Inputs for DCM
-extern float Gyro_Vector[3];
-extern float Accel_Vector[3];
+extern struct FloatRates gyro_float;
+extern struct FloatVect3 accel_float;
 
 // Integrate Inertial
 void Matrix_update(void);