diff --git a/conf/messages.xml b/conf/messages.xml
index 9a54c46d72..fdbff85508 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -1065,7 +1065,8 @@
   </message>
 
   <message name="BOOZ2_HFF_X" id="164">
-      <field name="measure"        type="float"/>
+      <field name="x_measure"      type="float"/>
+	  <field name="xdd"            type="float"/>
       <field name="x"              type="float"/>
       <field name="xd"             type="float"/>
       <field name="xbias"          type="float"/>
@@ -1075,7 +1076,8 @@
   </message>
 
   <message name="BOOZ2_HFF_Y" id="165">
-      <field name="measure"        type="float"/>
+      <field name="y_measure"      type="float"/>
+	  <field name="ydd"            type="float"/>
       <field name="y"              type="float"/>
       <field name="yd"             type="float"/>
       <field name="ybias"          type="float"/>
diff --git a/sw/airborne/booz/booz2_telemetry.h b/sw/airborne/booz/booz2_telemetry.h
index 5fd1035af1..7506fca5d5 100644
--- a/sw/airborne/booz/booz2_telemetry.h
+++ b/sw/airborne/booz/booz2_telemetry.h
@@ -457,6 +457,7 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 #define PERIODIC_SEND_BOOZ2_HFF_X(_chan) {		\
     DOWNLINK_SEND_BOOZ2_HFF_X(_chan,			\
 			    &b2_hff_x_meas,		\
+  	            &b2_hff_xdotdot,    \
 			    &b2_hff_x,			\
 			    &b2_hff_xdot,		\
 			    &b2_hff_xbias,		\
@@ -467,6 +468,7 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 #define PERIODIC_SEND_BOOZ2_HFF_Y(_chan) {		\
     DOWNLINK_SEND_BOOZ2_HFF_Y(_chan,			\
 			    &b2_hff_y_meas,		\
+	            &b2_hff_ydotdot,    \
 			    &b2_hff_y,			\
 			    &b2_hff_ydot,		\
 			    &b2_hff_ybias,		\
diff --git a/sw/airborne/booz/ins/booz2_hf_float.c b/sw/airborne/booz/ins/booz2_hf_float.c
index e9a5b6431e..65e1c26842 100644
--- a/sw/airborne/booz/ins/booz2_hf_float.c
+++ b/sw/airborne/booz/ins/booz2_hf_float.c
@@ -44,8 +44,8 @@ X_y = [ y ydot ybias ]
 #define Qdotdot ACCEL_NOISE*DT_HFILTER
 #define Qbiasbias 1e-7*HFF_PRESCALER
 //TODO: proper measurement noise
-#define Rpos   1.
-#define Rspeed 2.
+#define Rpos   5.
+#define Rspeed 1.
 
 float b2_hff_x;
 float b2_hff_xbias;
@@ -135,7 +135,7 @@ void b2_hff_propagate(float xaccel, float yaccel) {
 static inline void b2_hff_propagate_x(float xaccel) {
   /* update state */
   b2_hff_xdotdot = xaccel - b2_hff_xbias;
-  b2_hff_x = b2_hff_x + DT_HFILTER * b2_hff_xdot;
+  b2_hff_x = b2_hff_x + DT_HFILTER * b2_hff_xdot + DT_HFILTER * DT_HFILTER / 2 * b2_hff_xdotdot;
   b2_hff_xdot = b2_hff_xdot + DT_HFILTER * b2_hff_xdotdot;
   /* update covariance */
   const float FPF00 = b2_hff_xP[0][0] + DT_HFILTER * ( b2_hff_xP[1][0] + b2_hff_xP[0][1] + DT_HFILTER * b2_hff_xP[1][1] );