diff --git a/conf/simulator/nps/nps_sensors_params_booz2_a1.h b/conf/simulator/nps/nps_sensors_params_booz2_a1.h
index 34c9f1cbf2..718ab2d542 100644
--- a/conf/simulator/nps/nps_sensors_params_booz2_a1.h
+++ b/conf/simulator/nps/nps_sensors_params_booz2_a1.h
@@ -77,17 +77,17 @@
 #define NPS_GYRO_NEUTRAL_Q        33417
 #define NPS_GYRO_NEUTRAL_R        32809
 
-#define NPS_GYRO_NOISE_STD_DEV_P  RadOfDeg(0.5)
-#define NPS_GYRO_NOISE_STD_DEV_Q  RadOfDeg(0.5)
-#define NPS_GYRO_NOISE_STD_DEV_R  RadOfDeg(0.5)
+#define NPS_GYRO_NOISE_STD_DEV_P  RadOfDeg(0.)
+#define NPS_GYRO_NOISE_STD_DEV_Q  RadOfDeg(0.)
+#define NPS_GYRO_NOISE_STD_DEV_R  RadOfDeg(0.)
 
-#define NPS_GYRO_BIAS_INITIAL_P   RadOfDeg( 0.0)
-#define NPS_GYRO_BIAS_INITIAL_Q   RadOfDeg( 0.0)
-#define NPS_GYRO_BIAS_INITIAL_R   RadOfDeg( 0.0)
+#define NPS_GYRO_BIAS_INITIAL_P   RadOfDeg( 1.0)
+#define NPS_GYRO_BIAS_INITIAL_Q   RadOfDeg( 2.0)
+#define NPS_GYRO_BIAS_INITIAL_R   RadOfDeg( -1.0)
 
-#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.)
-#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.)
-#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.)
+#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.5)
+#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.5)
+#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.5)
 /* s */
 #define NPS_GYRO_DT (1./512.)
 
diff --git a/sw/airborne/math/pprz_algebra.h b/sw/airborne/math/pprz_algebra.h
index 902c9ef847..1431cf8fa7 100644
--- a/sw/airborne/math/pprz_algebra.h
+++ b/sw/airborne/math/pprz_algebra.h
@@ -161,6 +161,13 @@
     (_vo).z =  (_va).z / (_vb).z;				\
   } 
 
+/*  */
+#define VECT3_EW_MUL(_vo, _va, _vb) {				\
+    (_vo).x =  (_va).x * (_vb).x;				\
+    (_vo).y =  (_va).y * (_vb).y;				\
+    (_vo).z =  (_va).z * (_vb).z;				\
+  } 
+
 /*  */
 #define VECT3_BOUND_CUBE(_v, _min, _max) {				\
     if ((_v).x > (_max)) (_v).x = (_max); else if ((_v).x < (_min)) (_v).x = (_min); \
diff --git a/sw/simulator/nps/nps_random.c b/sw/simulator/nps/nps_random.c
index 315aa14d6e..1a8668106f 100644
--- a/sw/simulator/nps/nps_random.c
+++ b/sw/simulator/nps/nps_random.c
@@ -37,6 +37,23 @@ void double_vect3_add_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect
   vect->z += get_gaussian_noise() * std_dev->z;
 }
 
+void double_vect3_get_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev) {
+  vect->x = get_gaussian_noise() * std_dev->x;
+  vect->y = get_gaussian_noise() * std_dev->y;
+  vect->z = get_gaussian_noise() * std_dev->z;
+}
+
+
+void double_vect3_update_random_walk(struct DoubleVect3* rw, struct DoubleVect3* std_dev, double dt, double thau) {
+  struct DoubleVect3 drw;
+  double_vect3_get_gaussian_noise(&drw, std_dev);
+  struct DoubleVect3 tmp;
+  VECT3_SMUL(tmp, (-1./thau), *rw);
+  VECT3_ADD(drw, tmp);
+  VECT3_SMUL(drw, dt, drw);
+  VECT3_ADD(*rw, drw);
+}
+
 /* 
    http://www.taygeta.com/random/gaussian.html 
 */
diff --git a/sw/simulator/nps/nps_random.h b/sw/simulator/nps/nps_random.h
index 8a4bfa08df..bb430dfc05 100644
--- a/sw/simulator/nps/nps_random.h
+++ b/sw/simulator/nps/nps_random.h
@@ -5,8 +5,8 @@
 
 extern double get_gaussian_noise(void);
 extern void double_vect3_add_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev);
-
-
+extern void double_vect3_get_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev);
+extern void double_vect3_update_random_walk(struct DoubleVect3* rw, struct DoubleVect3* std_dev, double dt, double thau);
 
 #endif /* NPS_RANDOM_H */
 
diff --git a/sw/simulator/nps/nps_sensor_accel.c b/sw/simulator/nps/nps_sensor_accel.c
index 65e6245d4c..9766c452ea 100644
--- a/sw/simulator/nps/nps_sensor_accel.c
+++ b/sw/simulator/nps/nps_sensor_accel.c
@@ -8,7 +8,7 @@
 #include "math/pprz_algebra_int.h"
 
 void   nps_sensor_accel_init(struct NpsSensorAccel* accel, double time) {
-  VECT3_ASSIGN(accel->value, 0., 0., 0.);
+  FLOAT_VECT3_ZERO(accel->value);
   accel->resolution = NPS_ACCEL_RESOLUTION;
   FLOAT_MAT33_DIAG(accel->sensitivity, 
 		   NPS_ACCEL_SENSITIVITY_XX, NPS_ACCEL_SENSITIVITY_YY, NPS_ACCEL_SENSITIVITY_ZZ);
@@ -56,9 +56,8 @@ void   nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, stru
   /* white noise   */
   double_vect3_add_gaussian_noise(&accelero_error, &accel->noise_std_dev);
   /* scale */
-  accelero_error.x *= MAT33_ELMT(accel->sensitivity, 0, 0);
-  accelero_error.y *= MAT33_ELMT(accel->sensitivity, 1, 1);
-  accelero_error.z *= MAT33_ELMT(accel->sensitivity, 2, 2);
+  struct DoubleVect3 gain = {NPS_ACCEL_SENSITIVITY_XX, NPS_ACCEL_SENSITIVITY_YY, NPS_ACCEL_SENSITIVITY_ZZ};
+  VECT3_EW_MUL(accelero_error, accelero_error, gain);
   /* add error */
   VECT3_ADD(accel->value, accelero_error);
 
diff --git a/sw/simulator/nps/nps_sensor_gyro.c b/sw/simulator/nps/nps_sensor_gyro.c
index 86cece7471..9cffe9341b 100644
--- a/sw/simulator/nps/nps_sensor_gyro.c
+++ b/sw/simulator/nps/nps_sensor_gyro.c
@@ -4,9 +4,10 @@
 #include "nps_fdm.h"
 #include NPS_SENSORS_PARAMS
 #include "math/pprz_algebra_int.h"
+#include "nps_random.h"
 
 void  nps_sensor_gyro_init(struct NpsSensorGyro* gyro, double time) {
-  VECT3_ASSIGN(gyro->value, 0., 0., 0.);
+  FLOAT_VECT3_ZERO(gyro->value);
   gyro->resolution = NPS_GYRO_RESOLUTION;
   FLOAT_MAT33_DIAG(gyro->sensitivity, 
 		   NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR);
@@ -20,6 +21,7 @@ void  nps_sensor_gyro_init(struct NpsSensorGyro* gyro, double time) {
 	       NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P, 
 	       NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q, 
 	       NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R);
+  FLOAT_VECT3_ZERO(gyro->bias_random_walk_value);
   gyro->next_update = time;
   gyro->data_available = FALSE;
 }
@@ -30,43 +32,30 @@ void nps_sensor_gyro_run_step(struct NpsSensorGyro* gyro, double time, struct Do
     return;
 
   /* transform body rates to IMU frame */
-  DoubleVect3* rate_body = (struct DoubleVect3*)(&fdm.body_ecef_rotvel);
-  DoubleVect3 rate_imu;
+  struct DoubleVect3* rate_body = (struct DoubleVect3*)(&fdm.body_ecef_rotvel);
+  struct DoubleVect3 rate_imu;
   MAT33_VECT3_MUL(rate_imu, *body_to_imu, *rate_body );
   /* compute gyros readings */
   MAT33_VECT3_MUL(gyro->value, gyro->sensitivity, rate_imu);
   VECT3_ADD(gyro->value, gyro->neutral);
   /* compute gyro error readings */
+  struct DoubleVect3 gyro_error;
+  VECT3_COPY(gyro_error, gyro->bias_initial);
+  double_vect3_add_gaussian_noise(&gyro_error, &gyro->noise_std_dev);
+  double_vect3_update_random_walk(&gyro->bias_random_walk_value, &gyro->bias_random_walk_std_dev, 
+				  NPS_GYRO_DT, 5.);
+  VECT3_ADD(gyro_error, gyro->bias_random_walk_value);
+
+  struct DoubleVect3 gain = {NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR};
+  VECT3_EW_MUL(gyro_error, gyro_error, gain);
   
+  VECT3_ADD(gyro->value, gyro_error);
+
   /* round signal to account for adc discretisation */
   DOUBLE_VECT3_ROUND(gyro->value);
   /* saturate                                       */
   VECT3_BOUND_CUBE(gyro->value, 0, gyro->resolution); 
-  
-#if 0
-  /* compute gyro error readings */
-  static VEC *gyro_error = VNULL;
-  gyro_error = v_resize(gyro_error, AXIS_NB);
-  gyro_error = v_zero(gyro_error);
-  /* add a gaussian noise */
-  gyro_error = v_add_gaussian_noise(gyro_error, bsm.gyro_noise_std_dev, gyro_error);
-  /* update random walk bias */
-  bsm.gyro_bias_random_walk_value = 
-    v_update_random_walk(bsm.gyro_bias_random_walk_value, 
-  			 bsm.gyro_bias_random_walk_std_dev, BSM_GYRO_DT, 
-  			 bsm.gyro_bias_random_walk_value);
-  gyro_error = v_add(gyro_error, bsm.gyro_bias_random_walk_value, gyro_error); 
-  /* scale to adc units FIXME : should use full adc gain ? sum ? */
-  gyro_error->ve[AXIS_P] = 
-    gyro_error->ve[AXIS_P] * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P];
-  gyro_error->ve[AXIS_Q] = 
-    gyro_error->ve[AXIS_Q] * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q];
-  gyro_error->ve[AXIS_R] = 
-    gyro_error->ve[AXIS_R] * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R];
-  /* add per gyro error reading */
-  bsm.gyro =  v_add(bsm.gyro, gyro_error, bsm.gyro); 
-#endif
-
+ 
   gyro->next_update += NPS_GYRO_DT;
   gyro->data_available = TRUE;
 }