diff --git a/conf/autopilot/booz2_simulator_nps.makefile b/conf/autopilot/booz2_simulator_nps.makefile
index 19536e20b2..fece8f905d 100644
--- a/conf/autopilot/booz2_simulator_nps.makefile
+++ b/conf/autopilot/booz2_simulator_nps.makefile
@@ -26,10 +26,15 @@ sim.CFLAGS += -DJSBSIM_ROOT_DIR=\"/home/violato/enac/programs/JSBSim/\"
 
 sim.srcs = $(SIMDIR)/nps_main.c                      \
 	   $(SIMDIR)/nps_fdm_jsbsim.c                \
+	   $(SIMDIR)/nps_random.c                    \
+	   $(SIMDIR)/booz_r250.c                     \
+	   $(SIMDIR)/booz_randlcg.c                  \
 	   $(SIMDIR)/nps_sensors.c                   \
 	   $(SIMDIR)/nps_sensor_gyro.c               \
 	   $(SIMDIR)/nps_sensor_accel.c              \
 	   $(SIMDIR)/nps_sensor_mag.c                \
+	   $(SIMDIR)/nps_sensor_baro.c               \
+	   $(SIMDIR)/nps_sensor_gps.c                \
 	   $(SIMDIR)/nps_autopilot.c                 \
 
 sim.srcs += $(SRC_BOOZ)/booz_trig_int.c
diff --git a/conf/simulator/nps_sensors_params_booz2_a1.h b/conf/simulator/nps_sensors_params_booz2_a1.h
index 73240a25d1..2216a40dc6 100644
--- a/conf/simulator/nps_sensors_params_booz2_a1.h
+++ b/conf/simulator/nps_sensors_params_booz2_a1.h
@@ -126,10 +126,10 @@
  */
 /* m */
 /* aka 2^8/INS_BARO_SENS  */
-#define BSM_BARO_QNH             900.
-#define BSM_BARO_SENSITIVITY      17.066667
-#define BSM_BARO_DT          (1./100.)
-#define BSM_BARO_NOISE_STD_DEV     5.e-2
+#define NPS_BARO_QNH             900.
+#define NPS_BARO_SENSITIVITY      17.066667
+#define NPS_BARO_DT              (1./100.)
+#define NPS_BARO_NOISE_STD_DEV     5.e-2
 
 /*
  *  GPS
@@ -163,10 +163,6 @@
 
 #endif /* GPS_PERFECT */
 
-#define BSM_GPS_POS_INITIAL_UTM_EAST            37728000
-#define BSM_GPS_POS_INITIAL_UTM_NORTH          482464300
-#define BSM_GPS_POS_INITIAL_UTM_ALT                15200
-
-#define BSM_GPS_DT                           (1./4.)
+#define NPS_GPS_DT                           (1./4.)
 
 #endif /* NPS_SENSORS_PARAMS_H */
diff --git a/sw/simulator/nps_autopilot.c b/sw/simulator/nps_autopilot.c
index 1a4b6b8487..2e5bf983df 100644
--- a/sw/simulator/nps_autopilot.c
+++ b/sw/simulator/nps_autopilot.c
@@ -2,6 +2,8 @@
 
 struct NpsAutopilot autopilot;
 
+#include "nps_sensors.h"
+
 void nps_autopilot_init(void) {
 
   /* Just for testing fdm */
@@ -12,4 +14,13 @@ void nps_autopilot_init(void) {
   autopilot.commands[3] = 0.5;
 
 }
-void nps_autopilot_run_step(void) {}
+
+
+void nps_autopilot_run_step(void) {
+
+  if (nps_sensors_gyro_available()) {
+    //    booz2_imu_feed_data();
+    //    booz2_main_event();
+  }
+
+}
diff --git a/sw/simulator/nps_fdm.h b/sw/simulator/nps_fdm.h
index 4b357cb58d..7400d77b75 100644
--- a/sw/simulator/nps_fdm.h
+++ b/sw/simulator/nps_fdm.h
@@ -15,6 +15,8 @@ struct NpsFdm {
   struct EcefCoor_d  ecef_vel;
   struct DoubleVect3 body_accel;
 
+  struct NedCoor_d ltp_pos;
+
   struct DoubleQuat  ltp_to_body_quat;
   struct DoubleRates body_rate;
   struct DoubleRates body_rate_dot;
diff --git a/sw/simulator/nps_main.c b/sw/simulator/nps_main.c
index d950c34541..3457b126e8 100644
--- a/sw/simulator/nps_main.c
+++ b/sw/simulator/nps_main.c
@@ -66,20 +66,21 @@ static void nps_main_init(void) {
 static void nps_main_run_sim_step(void) {
 
 
-  printf("sim at %f\n", nps_main.sim_time);
+  //  printf("sim at %f\n", nps_main.sim_time);
 
   nps_fdm_run_step(autopilot.commands);
 
-  //  nps_sensors_run_step();
+  nps_sensors_run_step(nps_main.sim_time);
 
-  //  nps_autopilot_run_step();
+  nps_autopilot_run_step();
 
 }
 
 
 static void nps_main_display(void) {
-  printf("display at %f\n", nps_main.display_time);
+  //  printf("display at %f\n", nps_main.display_time);
   nps_ivy_display();
+  
 }
 
 
diff --git a/sw/simulator/nps_sensor_accel.c b/sw/simulator/nps_sensor_accel.c
index fbf31694e0..2d6ad526a0 100644
--- a/sw/simulator/nps_sensor_accel.c
+++ b/sw/simulator/nps_sensor_accel.c
@@ -2,8 +2,10 @@
 
 #include "airframe.h"
 #include "nps_fdm.h"
+#include "nps_random.h"
 #include NPS_SENSORS_PARAMS
 
+
 void   nps_sensor_accel_init(struct NpsSensorAccel* accel, double time) {
   VECT3_ASSIGN(accel->value, 0., 0., 0.);
   accel->resolution = NPS_ACCEL_RESOLUTION;
@@ -19,6 +21,7 @@ void   nps_sensor_accel_init(struct NpsSensorAccel* accel, double time) {
   accel->data_available = FALSE;
 }
 
+
 void   nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, struct DoubleRMat* body_to_imu) {
 
   if (time < accel->next_update)
@@ -39,8 +42,19 @@ void   nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, stru
   /* compute accelero readings */
   MAT33_VECT3_MUL(accel->value, accel->sensitivity, accelero_imu);
   
-  /* FIXME: ADD error reading */
-  
+  /* Compute sensor error */
+  struct DoubleVect3 accelero_error;
+  /* constant bias */
+  VECT3_COPY(accelero_error, accel->bias);
+  /* 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);
+  /* add error */
+  VECT3_ADD(accel->value, accelero_error);
+
   /* round signal to account for adc discretisation */
   DOUBLE_VECT3_ROUND(accel->value);
   /* saturate                                       */
diff --git a/sw/simulator/nps_sensor_baro.c b/sw/simulator/nps_sensor_baro.c
new file mode 100644
index 0000000000..fda2518a28
--- /dev/null
+++ b/sw/simulator/nps_sensor_baro.c
@@ -0,0 +1,31 @@
+#include "nps_sensor_baro.h"
+
+#include "airframe.h"
+#include "nps_fdm.h"
+#include "nps_random.h"
+#include NPS_SENSORS_PARAMS
+
+void nps_sensor_baro_init(struct NpsSensorBaro* baro, double time) {
+  baro->value = 0.;
+  baro->next_update = time;
+  baro->data_available = FALSE;
+}
+
+
+void nps_sensor_baro_run_step(struct NpsSensorBaro* baro, double time) {
+  
+  if (time < baro->next_update)
+    return;
+
+  if (time < 12.5)
+    baro->value = 840;
+  else {
+    double z = fdm.ltp_pos.z + get_gaussian_noise()*NPS_BARO_NOISE_STD_DEV;
+    double baro_reading = NPS_BARO_QNH + z * NPS_BARO_SENSITIVITY;
+    baro_reading = rint(baro_reading);
+    baro->value = baro_reading;
+  }
+  
+  baro->next_update += NPS_BARO_DT;
+  baro->data_available = TRUE;
+}
diff --git a/sw/simulator/nps_sensor_baro.h b/sw/simulator/nps_sensor_baro.h
new file mode 100644
index 0000000000..c0110a1749
--- /dev/null
+++ b/sw/simulator/nps_sensor_baro.h
@@ -0,0 +1,19 @@
+#ifndef NPS_SENSOR_BARO_H
+#define NPS_SENSOR_BARO_H
+
+#include "pprz_algebra.h"
+#include "pprz_algebra_double.h"
+#include "pprz_algebra_float.h"
+#include "std.h"
+
+struct NpsSensorBaro {
+  double  value;
+  double  next_update;
+  bool_t  data_available;
+};
+
+
+extern void nps_sensor_baro_init(struct NpsSensorBaro* baro, double time);
+extern void nps_sensor_baro_run_step(struct NpsSensorBaro* baro, double time);
+
+#endif /* NPS_SENSOR_BARO_H */
diff --git a/sw/simulator/nps_sensor_gps.c b/sw/simulator/nps_sensor_gps.c
new file mode 100644
index 0000000000..af4b3b204c
--- /dev/null
+++ b/sw/simulator/nps_sensor_gps.c
@@ -0,0 +1,28 @@
+#include "nps_sensor_gps.h"
+
+#include "airframe.h"
+#include "nps_fdm.h"
+#include "nps_random.h"
+#include NPS_SENSORS_PARAMS
+
+void nps_sensor_gps_init(struct NpsSensorGps* gps, double time) {
+  FLOAT_VECT3_ZERO(gps->ecef_pos);
+  FLOAT_VECT3_ZERO(gps->ecef_vel);
+  gps->next_update = time;
+  gps->data_available = FALSE;
+}
+
+
+void nps_sensor_gps_run_step(struct NpsSensorGps* gps, double time) {
+
+  if (time < gps->next_update)
+    return;
+
+  VECT3_COPY(gps->ecef_pos, fdm.ecef_pos);
+  VECT3_COPY(gps->ecef_vel, fdm.ecef_vel);
+
+  gps->next_update += NPS_GPS_DT;
+  gps->data_available = TRUE;
+
+}
+
diff --git a/sw/simulator/nps_sensor_gps.h b/sw/simulator/nps_sensor_gps.h
new file mode 100644
index 0000000000..c5bb4da8cc
--- /dev/null
+++ b/sw/simulator/nps_sensor_gps.h
@@ -0,0 +1,22 @@
+#ifndef NPS_SENSOR_GPS_H
+#define NPS_SENSOR_GPS_H
+
+#include "pprz_algebra.h"
+#include "pprz_algebra_double.h"
+#include "pprz_algebra_float.h"
+#include "pprz_geodetic_double.h"
+
+#include "std.h"
+
+struct NpsSensorGps {
+  struct EcefCoor_d ecef_pos;
+  struct EcefCoor_d ecef_vel;
+  double       next_update;
+  bool_t       data_available;
+};
+
+
+extern void nps_sensor_gps_init(struct NpsSensorGps* gps, double time);
+extern void nps_sensor_gps_run_step(struct NpsSensorGps* gps, double time);
+
+#endif /* NPS_SENSOR_GPS_H */
diff --git a/sw/simulator/nps_sensor_mag.h b/sw/simulator/nps_sensor_mag.h
index 1b424e4582..5066233cb8 100644
--- a/sw/simulator/nps_sensor_mag.h
+++ b/sw/simulator/nps_sensor_mag.h
@@ -21,4 +21,4 @@ struct NpsSensorMag {
 extern void nps_sensor_mag_init(struct NpsSensorMag* mag, double time);
 extern void nps_sensor_mag_run_step(struct NpsSensorMag* mag, double time, struct DoubleRMat* body_to_imu);
 
-#endif /* NPS_SENSOR_ACCEL_H */
+#endif /* NPS_SENSOR_MAG_H */
diff --git a/sw/simulator/nps_sensors.c b/sw/simulator/nps_sensors.c
index 80eb6f4f56..be26cc83ad 100644
--- a/sw/simulator/nps_sensors.c
+++ b/sw/simulator/nps_sensors.c
@@ -14,13 +14,18 @@ void nps_sensors_init(double time) {
   nps_sensor_gyro_init(&sensors.gyro, time);
   nps_sensor_accel_init(&sensors.accel, time);
   nps_sensor_mag_init(&sensors.mag, time);
+  nps_sensor_baro_init(&sensors.baro, time);
+  nps_sensor_gps_init(&sensors.gps, time);
 
 }
 
+
 void nps_sensors_run_step(double time) {
   nps_sensor_gyro_run_step(&sensors.gyro, time, &sensors.body_to_imu_rmat);
   nps_sensor_accel_run_step(&sensors.accel, time, &sensors.body_to_imu_rmat);
   nps_sensor_mag_run_step(&sensors.mag, time, &sensors.body_to_imu_rmat);
+  nps_sensor_baro_run_step(&sensors.baro, time);
+  nps_sensor_gps_run_step(&sensors.gps, time);
 }
 
 
@@ -39,3 +44,19 @@ bool_t nps_sensors_mag_available(void) {
   }
   return FALSE;
 }
+
+bool_t nps_sensors_baro_available(void) {
+  if (sensors.baro.data_available) {
+    sensors.baro.data_available = FALSE;
+    return TRUE;
+  }
+  return FALSE;
+}
+
+bool_t nps_sensors_gps_available(void) {
+  if (sensors.gps.data_available) {
+    sensors.gps.data_available = FALSE;
+    return TRUE;
+  }
+  return FALSE;
+}
diff --git a/sw/simulator/nps_sensors.h b/sw/simulator/nps_sensors.h
index 6de6fa47a2..0b4a49877d 100644
--- a/sw/simulator/nps_sensors.h
+++ b/sw/simulator/nps_sensors.h
@@ -5,8 +5,8 @@
 #include "nps_sensor_gyro.h"
 #include "nps_sensor_accel.h"
 #include "nps_sensor_mag.h"
-//#include "nps_sensor_baro.h"
-//#include "nps_sensor_gps.h"
+#include "nps_sensor_baro.h"
+#include "nps_sensor_gps.h"
 
 
 struct NpsSensors {
@@ -14,9 +14,8 @@ struct NpsSensors {
   struct NpsSensorGyro  gyro;
   struct NpsSensorAccel accel;
   struct NpsSensorMag   mag;
-  //  struct NpsSensorBaro  baro;
-  //  struct NpsSensorGps   gps;
-
+  struct NpsSensorBaro  baro;
+  struct NpsSensorGps   gps;
 };
 
 extern struct NpsSensors sensors;
@@ -26,5 +25,8 @@ extern void nps_sensors_run_step(double time);
 
 extern bool_t nps_sensors_gyro_available();
 extern bool_t nps_sensors_mag_available();
+extern bool_t nps_sensors_baro_available();
+extern bool_t nps_sensors_gps_available();
+
 
 #endif /* NPS_SENSORS_H */