diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile
index bc6345b920..40a88da4a0 100644
--- a/conf/firmwares/rotorcraft.makefile
+++ b/conf/firmwares/rotorcraft.makefile
@@ -167,8 +167,10 @@ ap.CFLAGS += -DUSE_ADC
 ap.srcs   += $(SRC_ARCH)/mcu_periph/adc_arch.c
 ap.srcs   += subsystems/electrical.c
 # baro has variable offset amplifier on booz board
+ifeq ($(BOARD), booz)
 ap.CFLAGS += -DUSE_DAC
 ap.srcs   += $(SRC_ARCH)/mcu_periph/dac_arch.c
+endif
 else ifeq ($(ARCH), stm32)
 ap.CFLAGS += -DUSE_ADC
 ap.CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4
@@ -208,19 +210,16 @@ ap.CFLAGS += -DUSE_NAVIGATION
 ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c
 ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c
 
-ap.srcs += $(SRC_SUBSYSTEMS)/ins.c
-
 #
 # INS choice
 #
-# include subsystems/rotorcraft/ins_hff.makefile
+# include subsystems/rotorcraft/ins.makefile
 # or
-# nothing
+# include subsystems/rotorcraft/ins_extended.makefile
+#
+# extra:
+# include subsystems/rotorcraft/ins_hff.makefile
 #
-
-#  vertical filter float version
-ap.srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c
-ap.CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./$(PERIODIC_FREQUENCY).)'
 
 ap.srcs += $(SRC_FIRMWARE)/navigation.c
 ap.srcs += subsystems/navigation/common_flight_plan.c
diff --git a/conf/firmwares/subsystems/rotorcraft/ins.makefile b/conf/firmwares/subsystems/rotorcraft/ins.makefile
new file mode 100644
index 0000000000..7fb608ffef
--- /dev/null
+++ b/conf/firmwares/subsystems/rotorcraft/ins.makefile
@@ -0,0 +1,10 @@
+#
+# simple INS with float vertical filter
+#
+
+ap.srcs += $(SRC_SUBSYSTEMS)/ins.c
+
+#  vertical filter float version
+ap.srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c
+ap.CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./$(PERIODIC_FREQUENCY).)'
+
diff --git a/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile b/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile
new file mode 100644
index 0000000000..f1cd021b44
--- /dev/null
+++ b/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile
@@ -0,0 +1,10 @@
+#
+# extended INS with vertical filter using sonar in a better way (flap ground)
+#
+
+ap.srcs += $(SRC_SUBSYSTEMS)/ins_extended.c
+
+#  vertical filter float version
+ap.srcs += $(SRC_SUBSYSTEMS)/ins/vf_extended_float.c
+ap.CFLAGS += -DUSE_VFF_EXTENDED -DDT_VFILTER='(1./$(PERIODIC_FREQUENCY).)'
+
diff --git a/conf/messages.xml b/conf/messages.xml
index cf25f32bc5..1139d855af 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -1334,6 +1334,16 @@
   </message>
 
   <!-- 161 is free -->
+  <message name="VFF_EXTENDED" id="161">
+      <field name="measure1"        type="float"/>
+      <field name="measure2"        type="float"/>
+      <field name="z"              type="float"/>
+      <field name="zd"             type="float"/>
+      <field name="zdd"             type="float"/>
+      <field name="bias"           type="float"/>
+      <field name="offset"           type="float"/>
+  </message>
+
 
   <message name="VFF" id="162">
       <field name="measure"        type="float"/>
diff --git a/conf/settings/estimation/ins_sonar.xml b/conf/settings/estimation/ins_sonar.xml
new file mode 100644
index 0000000000..0c016fa593
--- /dev/null
+++ b/conf/settings/estimation/ins_sonar.xml
@@ -0,0 +1,9 @@
+<!DOCTYPE settings SYSTEM "../settings.dtd">
+
+<settings>
+  <dl_settings>
+    <dl_settings NAME="sonar">
+      <dl_setting MAX="1" MIN="0" STEP="1" VAR="ins_update_on_agl" shortname="use_sonar" values="FALSE|TRUE" module="subsystems/ins/vf_extended_float"/>
+    </dl_settings>
+  </dl_settings>
+</settings>
diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h
index 80502d42de..28901e5288 100644
--- a/sw/airborne/firmwares/rotorcraft/telemetry.h
+++ b/sw/airborne/firmwares/rotorcraft/telemetry.h
@@ -495,6 +495,21 @@
 #define PERIODIC_SEND_VFF(_trans, _dev) {}
 #endif
 
+#if USE_VFF_EXTENDED
+#include "subsystems/ins/vf_extended_float.h"
+#define PERIODIC_SEND_VFF_EXTENDED(_trans, _dev) {		\
+    DOWNLINK_SEND_VFF_EXTENDED(_trans, _dev,			\
+                &vff_z_meas,		\
+                &vff_z_meas_baro, \
+                &vff_z,			\
+                &vff_zdot,		\
+                &vff_bias,		\
+                &vff_offset);		\
+  }
+#else
+#define PERIODIC_SEND_VFF_EXTENDED(_trans, _dev) {}
+#endif
+
 #if USE_HFF
 #include  "subsystems/ins/hf_float.h"
 #define PERIODIC_SEND_HFF(_trans, _dev) {	\
diff --git a/sw/airborne/subsystems/ins.h b/sw/airborne/subsystems/ins.h
index 83bb470096..a3f876031d 100644
--- a/sw/airborne/subsystems/ins.h
+++ b/sw/airborne/subsystems/ins.h
@@ -33,7 +33,7 @@ extern struct NedCoor_i ins_gps_pos_cm_ned;
 extern struct NedCoor_i ins_gps_speed_cm_s_ned;
 
 /* barometer                   */
-#if USE_VFF
+#if USE_VFF || USE_VFF_EXTENDED
 extern int32_t ins_baro_alt;
 extern int32_t ins_qfe;
 extern bool_t  ins_baro_initialised;
diff --git a/sw/airborne/subsystems/ins/vf_extended_float.c b/sw/airborne/subsystems/ins/vf_extended_float.c
new file mode 100644
index 0000000000..4013c4fd59
--- /dev/null
+++ b/sw/airborne/subsystems/ins/vf_extended_float.c
@@ -0,0 +1,313 @@
+/*
+ * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
+ * Copyright (C) 2012 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#include "subsystems/ins/vf_extended_float.h"
+
+#define DEBUG_VFF_EXTENDED 1
+
+#if DEBUG_VFF_EXTENDED
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "subsystems/datalink/downlink.h"
+#endif
+
+/*
+
+X = [ z zdot accel_bias baro_offset ]
+
+temps :
+  propagate 86us
+  update    46us
+
+*/
+/* initial covariance diagonal */
+#define INIT_PXX 1.
+/* process noise */
+#define ACCEL_NOISE 0.5
+#define Qzz       ACCEL_NOISE/512./512./2.
+#define Qzdotzdot ACCEL_NOISE/512.
+#define Qbiasbias 1e-7
+#define Qoffoff 1e-4
+#define R_BARO 1.
+#define R_ALT 0.1
+#define R_OFFSET 1.
+
+float vff_z;
+float vff_zdot;
+float vff_bias;
+float vff_offset;
+float vff_zdotdot;
+
+float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE];
+
+float vff_z_meas;
+float vff_z_meas_baro;
+
+void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_baro_offset) {
+  vff_z = init_z;
+  vff_zdot = init_zdot;
+  vff_bias = init_accel_bias;
+  vff_offset = init_baro_offset;
+  int i, j;
+  for (i=0; i<VFF_STATE_SIZE; i++) {
+    for (j=0; j<VFF_STATE_SIZE; j++)
+      vff_P[i][j] = 0.;
+    vff_P[i][i] = INIT_PXX;
+  }
+
+}
+
+
+/*
+
+ F = [ 1 dt -dt^2/2 0
+       0  1 -dt     0
+       0  0   1     0
+       0  0   0     1 ];
+
+ B = [ dt^2/2 dt 0 0]';
+
+ Q = [ Qzz   0          0         0
+       0     Qzdotzdot  0         0
+       0     0          Qbiasbias 0
+       0     0     0    0         Qoffoff ];
+
+ Xk1 = F * Xk0 + B * accel;
+
+ Pk1 = F * Pk0 * F' + Q;
+
+*/
+void vff_propagate(float accel) {
+  /* update state */
+  vff_zdotdot = accel + 9.81 - vff_bias;
+  vff_z = vff_z + DT_VFILTER * vff_zdot;
+  vff_zdot = vff_zdot + DT_VFILTER * vff_zdotdot;
+  /* update covariance */
+  const float FPF00 = vff_P[0][0] + DT_VFILTER * ( vff_P[1][0] + vff_P[0][1] + DT_VFILTER * vff_P[1][1] );
+  const float FPF01 = vff_P[0][1] + DT_VFILTER * ( vff_P[1][1] - vff_P[0][2] - DT_VFILTER * vff_P[1][2] );
+  const float FPF02 = vff_P[0][2] + DT_VFILTER * ( vff_P[1][2] );
+  const float FPF10 = vff_P[1][0] + DT_VFILTER * (-vff_P[2][0] + vff_P[1][1] - DT_VFILTER * vff_P[2][1] );
+  const float FPF11 = vff_P[1][1] + DT_VFILTER * (-vff_P[2][1] - vff_P[1][2] + DT_VFILTER * vff_P[2][2] );
+  const float FPF12 = vff_P[1][2] + DT_VFILTER * (-vff_P[2][2] );
+  const float FPF20 = vff_P[2][0] + DT_VFILTER * ( vff_P[2][1] );
+  const float FPF21 = vff_P[2][1] + DT_VFILTER * (-vff_P[2][2] );
+  const float FPF22 = vff_P[2][2];
+  const float FPF33 = vff_P[3][3];
+
+  vff_P[0][0] = FPF00 + Qzz;
+  vff_P[0][1] = FPF01;
+  vff_P[0][2] = FPF02;
+  vff_P[1][0] = FPF10;
+  vff_P[1][1] = FPF11 + Qzdotzdot;
+  vff_P[1][2] = FPF12;
+  vff_P[2][0] = FPF20;
+  vff_P[2][1] = FPF21;
+  vff_P[2][2] = FPF22 + Qbiasbias;
+  vff_P[3][3] = FPF33 + Qoffoff;
+
+#if DEBUG_VFF_EXTENDED
+  RunOnceEvery(10, DOWNLINK_SEND_VFF_EXTENDED(DefaultChannel, DefaultDevice, &vff_z_meas, &vff_z_meas_baro, &vff_z, &vff_zdot, &vff_zdotdot, &vff_bias, &vff_offset));
+#endif
+}
+
+/*
+ * Update sensor "with" offset (baro)
+  H = [1 0 0 -1];
+  // state residual
+  y = rangemeter - H * Xm;
+  // covariance residual
+  S = H*Pm*H' + R;
+  // kalman gain
+  K = Pm*H'*inv(S);
+  // update state
+  Xp = Xm + K*y;
+  // update covariance
+  Pp = Pm - K*H*Pm;
+*/
+__attribute__ ((always_inline)) static inline void update_baro_conf(float z_meas, float conf) {
+  vff_z_meas_baro = z_meas;
+
+  const float y = z_meas - vff_z + vff_offset;
+  const float S = vff_P[0][0] - vff_P[0][3] - vff_P[3][0] + vff_P[3][3] + conf;
+  const float K0 = (vff_P[0][0] - vff_P[0][3]) * 1/S;
+  const float K1 = (vff_P[1][0] - vff_P[1][3]) * 1/S;
+  const float K2 = (vff_P[2][0] - vff_P[2][3]) * 1/S;
+  const float K3 = (vff_P[3][0] - vff_P[3][3]) * 1/S;
+
+  vff_z       = vff_z       + K0 * y;
+  vff_zdot    = vff_zdot    + K1 * y;
+  vff_bias    = vff_bias    + K2 * y;
+  vff_offset  = vff_offset  + K3 * y;
+
+  const float P0 = vff_P[0][0] - vff_P[3][0];
+  const float P1 = vff_P[0][1] - vff_P[3][1];
+  const float P2 = vff_P[0][2] - vff_P[3][2];
+  const float P3 = vff_P[0][3] - vff_P[3][3];
+
+  vff_P[0][0] -= K0 * P0;
+  vff_P[0][1] -= K0 * P1;
+  vff_P[0][2] -= K0 * P2;
+  vff_P[0][3] -= K0 * P3;
+  vff_P[1][0] -= K1 * P0;
+  vff_P[1][1] -= K1 * P1;
+  vff_P[1][2] -= K1 * P2;
+  vff_P[1][3] -= K1 * P3;
+  vff_P[2][0] -= K2 * P0;
+  vff_P[2][1] -= K2 * P1;
+  vff_P[2][2] -= K2 * P2;
+  vff_P[2][3] -= K2 * P3;
+  vff_P[3][0] -= K3 * P0;
+  vff_P[3][1] -= K3 * P1;
+  vff_P[3][2] -= K3 * P2;
+  vff_P[3][3] -= K3 * P3;
+
+}
+
+void vff_update_baro(float z_meas) {
+  update_baro_conf(z_meas, R_BARO);
+}
+
+void vff_update_baro_conf(float z_meas, float conf) {
+  update_baro_conf(z_meas, conf);
+}
+
+/*
+ * Update sensor "without" offset (gps, sonar)
+  H = [1 0 0 0];
+  // state residual
+  y = rangemeter - H * Xm;
+  // covariance residual
+  S = H*Pm*H' + R;
+  // kalman gain
+  K = Pm*H'*inv(S);
+  // update state
+  Xp = Xm + K*y;
+  // update covariance
+  Pp = Pm - K*H*Pm;
+*/
+__attribute__ ((always_inline)) static inline void update_alt_conf(float z_meas, float conf) {
+  vff_z_meas = z_meas;
+
+  const float y = z_meas - vff_z;
+  const float S = vff_P[0][0] + conf;
+  const float K0 = vff_P[0][0] * 1/S;
+  const float K1 = vff_P[1][0] * 1/S;
+  const float K2 = vff_P[2][0] * 1/S;
+  const float K3 = vff_P[3][0] * 1/S;
+
+  vff_z       = vff_z       + K0 * y;
+  vff_zdot    = vff_zdot    + K1 * y;
+  vff_bias    = vff_bias    + K2 * y;
+  vff_offset  = vff_offset  + K3 * y;
+
+  const float P0 = vff_P[0][0];
+  const float P1 = vff_P[0][1];
+  const float P2 = vff_P[0][2];
+  const float P3 = vff_P[0][3];
+
+  vff_P[0][0] -= K0 * P0;
+  vff_P[0][1] -= K0 * P1;
+  vff_P[0][2] -= K0 * P2;
+  vff_P[0][3] -= K0 * P3;
+  vff_P[1][0] -= K1 * P0;
+  vff_P[1][1] -= K1 * P1;
+  vff_P[1][2] -= K1 * P2;
+  vff_P[1][3] -= K1 * P3;
+  vff_P[2][0] -= K2 * P0;
+  vff_P[2][1] -= K2 * P1;
+  vff_P[2][2] -= K2 * P2;
+  vff_P[2][3] -= K2 * P3;
+  vff_P[3][0] -= K3 * P0;
+  vff_P[3][1] -= K3 * P1;
+  vff_P[3][2] -= K3 * P2;
+  vff_P[3][3] -= K3 * P3;
+}
+
+void vff_update_alt(float z_meas) {
+  update_alt_conf(z_meas, R_ALT);
+}
+
+void vff_update_alt_conf(float z_meas, float conf) {
+  update_alt_conf(z_meas, conf);
+}
+
+/*
+ * Update sensor offset (baro)
+  H = [0 0 0 1];
+  // state residual
+  y = rangemeter - H * Xm;
+  // covariance residual
+  S = H*Pm*H' + R;
+  // kalman gain
+  K = Pm*H'*inv(S);
+  // update state
+  Xp = Xm + K*y;
+  // update covariance
+  Pp = Pm - K*H*Pm;
+*/
+__attribute__ ((always_inline)) static inline void update_offset_conf(float offset, float conf) {
+  const float y = offset - vff_offset;
+  const float S = vff_P[3][3] + conf;
+  const float K0 = vff_P[0][3] * 1/S;
+  const float K1 = vff_P[1][3] * 1/S;
+  const float K2 = vff_P[2][3] * 1/S;
+  const float K3 = vff_P[3][3] * 1/S;
+
+  vff_z       = vff_z       + K0 * y;
+  vff_zdot    = vff_zdot    + K1 * y;
+  vff_bias    = vff_bias    + K2 * y;
+  vff_offset  = vff_offset  + K3 * y;
+
+  const float P0 = vff_P[3][0];
+  const float P1 = vff_P[3][1];
+  const float P2 = vff_P[3][2];
+  const float P3 = vff_P[3][3];
+
+  vff_P[0][0] -= K0 * P0;
+  vff_P[0][1] -= K0 * P1;
+  vff_P[0][2] -= K0 * P2;
+  vff_P[0][3] -= K0 * P3;
+  vff_P[1][0] -= K1 * P0;
+  vff_P[1][1] -= K1 * P1;
+  vff_P[1][2] -= K1 * P2;
+  vff_P[1][3] -= K1 * P3;
+  vff_P[2][0] -= K2 * P0;
+  vff_P[2][1] -= K2 * P1;
+  vff_P[2][2] -= K2 * P2;
+  vff_P[2][3] -= K2 * P3;
+  vff_P[3][0] -= K3 * P0;
+  vff_P[3][1] -= K3 * P1;
+  vff_P[3][2] -= K3 * P2;
+  vff_P[3][3] -= K3 * P3;
+}
+
+void vff_update_offset(float offset) {
+  update_offset_conf(offset, R_OFFSET);
+}
+
+
+void vff_realign(float z_meas) {
+  //vff_z = z_meas;
+  //vff_zdot = 0.;
+  //vff_offset = 0.;
+  vff_init(z_meas, 0., 0., 0.);
+}
diff --git a/sw/airborne/subsystems/ins/vf_extended_float.h b/sw/airborne/subsystems/ins/vf_extended_float.h
new file mode 100644
index 0000000000..3f8b93b75e
--- /dev/null
+++ b/sw/airborne/subsystems/ins/vf_extended_float.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
+ * Copyright (C) 2012 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#ifndef VF_EXTENDED_FLOAT_H
+#define VF_EXTENDED_FLOAT_H
+
+#define VFF_STATE_SIZE 4
+
+extern float vff_z;
+extern float vff_zdot;
+extern float vff_bias;
+extern float vff_offset;
+extern float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE];
+extern float vff_zdotdot;
+
+extern float vff_z_meas;
+extern float vff_z_meas_baro;
+
+extern void vff_init(float z, float zdot, float accel_bias, float baro_offset);
+extern void vff_propagate(float accel);
+extern void vff_update_baro(float z_meas);
+extern void vff_update_alt(float z_meas);
+extern void vff_update_offset(float offset);
+extern void vff_update_baro_conf(float z_meas, float conf);
+extern void vff_update_alt_conf(float z_meas, float conf);
+//extern void vff_update_vz_conf(float vz_meas, float conf);
+extern void vff_realign(float z_meas);
+
+#endif /* VF_EXTENDED_FLOAT_H */
diff --git a/sw/airborne/subsystems/ins_extended.c b/sw/airborne/subsystems/ins_extended.c
new file mode 100644
index 0000000000..98ee7d53cd
--- /dev/null
+++ b/sw/airborne/subsystems/ins_extended.c
@@ -0,0 +1,299 @@
+/*
+ * Copyright (C) 2008-2010 The Paparazzi Team
+ * Copyright (C) 2012 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#include "subsystems/ins.h"
+
+#include "subsystems/imu.h"
+#include "subsystems/sensors/baro.h"
+#include "subsystems/gps.h"
+
+#include "generated/airframe.h"
+#include "math/pprz_algebra_int.h"
+#include "math/pprz_algebra_float.h"
+
+#include "subsystems/ahrs.h"
+
+#if USE_VFF_EXTENDED
+#include "subsystems/ins/vf_extended_float.h"
+#endif
+
+#if USE_HFF
+#include "subsystems/ins/hf_float.h"
+#endif
+
+#ifdef SITL
+#include "nps_fdm.h"
+#include <stdio.h>
+#endif
+
+
+#include "math/pprz_geodetic_int.h"
+
+#include "generated/flight_plan.h"
+
+/* gps transformed to LTP-NED  */
+struct LtpDef_i  ins_ltp_def;
+         bool_t  ins_ltp_initialised;
+struct NedCoor_i ins_gps_pos_cm_ned;
+struct NedCoor_i ins_gps_speed_cm_s_ned;
+#if USE_HFF
+/* horizontal gps transformed to NED in meters as float */
+struct FloatVect2 ins_gps_pos_m_ned;
+struct FloatVect2 ins_gps_speed_m_s_ned;
+#endif
+bool_t ins_hf_realign;
+
+/* barometer                   */
+#if USE_VFF_EXTENDED
+int32_t ins_qfe;
+bool_t  ins_baro_initialised;
+int32_t ins_baro_alt;
+#include "filters/median_filter.h"
+struct MedianFilterInt baro_median;
+#if USE_SONAR
+bool_t  ins_update_on_agl;
+int32_t ins_sonar_offset;
+struct MedianFilterInt sonar_median;
+#define VFF_R_SONAR_0 0.1
+#define VFF_R_SONAR_OF_M 0.2
+#endif
+#endif
+bool_t  ins_vf_realign;
+
+/* output                      */
+struct NedCoor_i ins_ltp_pos;
+struct NedCoor_i ins_ltp_speed;
+struct NedCoor_i ins_ltp_accel;
+struct EnuCoor_i ins_enu_pos;
+struct EnuCoor_i ins_enu_speed;
+struct EnuCoor_i ins_enu_accel;
+
+
+void ins_init() {
+#if USE_INS_NAV_INIT
+  ins_ltp_initialised = TRUE;
+
+  /** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */
+  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
+  llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0);
+  llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0);
+  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
+  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
+
+  struct EcefCoor_i ecef_nav0;
+  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
+
+  ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0);
+  ins_ltp_def.hmsl = NAV_ALT0;
+#else
+  ins_ltp_initialised  = FALSE;
+#endif
+#if USE_VFF_EXTENDED
+  ins_baro_initialised = FALSE;
+  init_median_filter(&baro_median);
+#if USE_SONAR
+  ins_update_on_agl = FALSE;
+  init_median_filter(&sonar_median);
+  ins_sonar_offset = 0;
+#endif
+  vff_init(0., 0., 0., 0.);
+#endif
+  ins_vf_realign = FALSE;
+  ins_hf_realign = FALSE;
+#if USE_HFF
+  b2_hff_init(0., 0., 0., 0.);
+#endif
+  INT32_VECT3_ZERO(ins_ltp_pos);
+  INT32_VECT3_ZERO(ins_ltp_speed);
+  INT32_VECT3_ZERO(ins_ltp_accel);
+  INT32_VECT3_ZERO(ins_enu_pos);
+  INT32_VECT3_ZERO(ins_enu_speed);
+  INT32_VECT3_ZERO(ins_enu_accel);
+}
+
+void ins_periodic( void ) {
+}
+
+#if USE_HFF
+void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) {
+  b2_hff_realign(pos, speed);
+}
+#else
+void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {}
+#endif /* USE_HFF */
+
+
+void ins_realign_v(float z) {
+#if USE_VFF_EXTENDED
+  vff_realign(z);
+#endif
+}
+
+void ins_propagate() {
+  /* untilt accels */
+  struct Int32Vect3 accel_meas_body;
+  INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
+  struct Int32Vect3 accel_meas_ltp;
+  INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, ahrs.ltp_to_body_rmat, accel_meas_body);
+
+#if USE_VFF_EXTENDED
+  float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
+  if (baro.status == BS_RUNNING && ins_baro_initialised) {
+    vff_propagate(z_accel_meas_float);
+    ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
+    ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
+    ins_ltp_pos.z   = POS_BFP_OF_REAL(vff_z);
+  }
+  else { // feed accel from the sensors
+    // subtract -9.81m/s2 (acceleration measured due to gravity, but vehivle not accelerating in ltp)
+    ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
+  }
+#if !USE_SONAR
+  vff_update_offset(vff_offset);
+#endif
+#else
+  ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
+#endif /* USE_VFF_EXTENDED */
+
+#if USE_HFF
+  /* propagate horizontal filter */
+  b2_hff_propagate();
+#else
+  ins_ltp_accel.x = accel_meas_ltp.x;
+  ins_ltp_accel.y = accel_meas_ltp.y;
+#endif /* USE_HFF */
+
+  INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);
+  INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed);
+  INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel);
+}
+
+void ins_update_baro() {
+#if USE_VFF_EXTENDED
+  int32_t baro_pressure = update_median_filter(&baro_median, baro.absolute);
+  if (baro.status == BS_RUNNING) {
+    if (!ins_baro_initialised) {
+      ins_qfe = baro_pressure;
+      ins_baro_initialised = TRUE;
+    }
+    if (ins_vf_realign) {
+      ins_vf_realign = FALSE;
+      ins_qfe = baro.absolute;
+#if USE_SONAR
+      // FIXME should use an averaged value
+      //ins_sonar_offset = sonar_meas;
+#endif
+      vff_realign(0.);
+      ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
+      ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
+      ins_ltp_pos.z   = POS_BFP_OF_REAL(vff_z);
+      ins_enu_pos.z = -ins_ltp_pos.z;
+      ins_enu_speed.z = -ins_ltp_speed.z;
+      ins_enu_accel.z = -ins_ltp_accel.z;
+    }
+    else { /* not realigning, so normal update with baro measurement */
+      ins_baro_alt = ((baro_pressure - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN;
+      float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt);
+      vff_update_baro(alt_float);
+    }
+  }
+#endif
+}
+
+
+void ins_update_gps(void) {
+#if USE_GPS
+  if (gps.fix == GPS_FIX_3D) {
+    if (!ins_ltp_initialised) {
+      ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos);
+      ins_ltp_def.lla.alt = gps.lla_pos.alt;
+      ins_ltp_def.hmsl = gps.hmsl;
+      ins_ltp_initialised = TRUE;
+    }
+    ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos);
+    ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel);
+#if USE_HFF
+    VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y);
+    VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.);
+    VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y);
+    VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.);
+    if (ins_hf_realign) {
+      ins_hf_realign = FALSE;
+#ifdef SITL
+      struct FloatVect2 true_pos, true_speed;
+      VECT2_COPY(true_pos, fdm.ltpprz_pos);
+      VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel);
+      b2_hff_realign(true_pos, true_speed);
+#else
+      const struct FloatVect2 zero = {0.0, 0.0};
+      b2_hff_realign(ins_gps_pos_m_ned, zero);
+#endif
+    }
+    b2_hff_update_gps();
+#if !USE_VFF_EXTENDED /* vff not used */
+    ins_ltp_pos.z =  (ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
+    ins_ltp_speed.z =  (ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN;
+#endif /* vff not used */
+#endif /* hff used */
+
+
+#if !USE_HFF /* hff not used */
+#if !USE_VFF_EXTENDED /* neither hf nor vf used */
+    INT32_VECT3_SCALE_3(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
+    INT32_VECT3_SCALE_3(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
+#else /* only vff used */
+    INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
+    INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
+#endif
+
+#if USE_GPS_LAG_HACK
+    VECT2_COPY(d_pos, ins_ltp_speed);
+    INT32_VECT2_RSHIFT(d_pos, d_pos, 11);
+    VECT2_ADD(ins_ltp_pos, d_pos);
+#endif
+#endif /* hff not used */
+
+    INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);
+    INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed);
+    INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel);
+  }
+#endif /* USE_GPS */
+}
+
+void ins_update_sonar() {
+#if USE_SONAR && USE_VFF_EXTENDED
+  static int32_t last_offset = 0;
+  //static int32_t sonar_filtered = 0;
+  int32_t sonar_filtered = update_median_filter(&sonar_median, sonar_meas);
+  //sonar_filtered = (sm + 2*sonar_filtered) / 3;
+  float sonar = (sonar_filtered - ins_sonar_offset) * INS_SONAR_SENS;
+  /* update filter assuming a flat ground */
+  if (sonar < INS_SONAR_MAX_RANGE && ins_update_on_agl && baro.status == BS_RUNNING) {
+    vff_update_alt_conf(-sonar, VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabs(sonar));
+    last_offset = vff_offset;
+  }
+  else {
+    /* update offset with last value to avoid divergence */
+    vff_update_offset(last_offset);
+  }
+#endif
+}