diff --git a/conf/airframes/booz2_a1.xml b/conf/airframes/booz2_a1.xml
index 280a5d4e16..50c1ff400f 100644
--- a/conf/airframes/booz2_a1.xml
+++ b/conf/airframes/booz2_a1.xml
@@ -109,6 +109,10 @@
   </section>
 
 
+  <section name="INS" prefix="BOOZ_INS_">
+    <define name="BARO_SENS" value="18.1" integer="16"/>
+  </section>
+
   <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
    <define name="HOVER_POWER" value="81"/>
    <define name="HOVER_KP"    value="-200"/>
@@ -132,8 +136,8 @@
 
  <section name="AUTOPILOT">
    <define name="BOOZ2_MODE_MANUAL" value="BOOZ2_AP_MODE_ATTITUDE_DIRECT"/>
-   <define name="BOOZ2_MODE_AUTO1" value="BOOZ2_AP_MODE_ATTITUDE_Z_HOLD"/>
-   <define name="BOOZ2_MODE_AUTO2" value="BOOZ2_AP_MODE_HOVER_Z_HOLD"/>
+   <define name="BOOZ2_MODE_AUTO1"  value="BOOZ2_AP_MODE_ATTITUDE_Z_HOLD"/>
+   <define name="BOOZ2_MODE_AUTO2"  value="BOOZ2_AP_MODE_HOVER_Z_HOLD"/>
  </section>
 
  <section name="FMS">
diff --git a/conf/airframes/booz2_a2.xml b/conf/airframes/booz2_a2.xml
index e0c7b4a78a..90625db664 100644
--- a/conf/airframes/booz2_a2.xml
+++ b/conf/airframes/booz2_a2.xml
@@ -91,12 +91,15 @@
    <define name="ALIGNMENT_DPSI"   value="BOOZ_ANGLE_I_OF_F(0.)"/>
   </section>
 
+  <section name="INS" prefix="BOOZ_INS_">
+    <define name="BARO_SENS" value="18.1" integer="16"/>
+  </section>
 
   <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
    <define name="HOVER_POWER" value="81"/>
-   <define name="HOVER_KP"    value="-200"/>
-   <define name="HOVER_KD"    value="-100"/>
-   <define name="HOVER_KI"    value="-50"/>
+   <define name="HOVER_KP"    value="-300"/>
+   <define name="HOVER_KD"    value="-200"/>
+   <define name="HOVER_KI"    value="-75"/>
   </section>
 
 
@@ -108,8 +111,8 @@
 
  <section name="AUTOPILOT">
    <define name="BOOZ2_MODE_MANUAL" value="BOOZ2_AP_MODE_ATTITUDE_DIRECT"/>
-   <define name="BOOZ2_MODE_AUTO1" value="BOOZ2_AP_MODE_ATTITUDE_Z_HOLD"/>
-   <define name="BOOZ2_MODE_AUTO2" value="BOOZ2_AP_MODE_ATTITUDE_Z_HOLD"/>
+   <define name="BOOZ2_MODE_AUTO1"  value="BOOZ2_AP_MODE_ATTITUDE_Z_HOLD"/>
+   <define name="BOOZ2_MODE_AUTO2"  value="BOOZ2_AP_MODE_ATTITUDE_Z_HOLD"/>
  </section>
 
  <section name="BAT">
diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml
index 2d97d9dd3d..e2ef080720 100644
--- a/conf/airframes/booz2_flixr.xml
+++ b/conf/airframes/booz2_flixr.xml
@@ -114,6 +114,9 @@
    <define name="ALIGNMENT_DPSI"   value="BOOZ_ANGLE_I_OF_F(0.)"/>
   </section>
 
+  <section name="INS" prefix="BOOZ_INS_">
+    <define name="BARO_SENS" value="18.1" integer="16"/>
+  </section>
 
   <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
    <define name="HOVER_POWER" value="86"/>
diff --git a/conf/airframes/booz2_j1.xml b/conf/airframes/booz2_j1.xml
index ef05ba623f..20d3929f5c 100644
--- a/conf/airframes/booz2_j1.xml
+++ b/conf/airframes/booz2_j1.xml
@@ -109,12 +109,15 @@
    <define name="ALIGNMENT_DPSI" value="BOOZ_ANGLE_I_OF_F(0)"/>
   </section>
 
+  <section name="INS" prefix="BOOZ_INS_">
+    <define name="BARO_SENS" value="18.1" integer="16"/>
+  </section>
 
   <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
    <define name="HOVER_POWER" value="95"/>
-   <define name="HOVER_KP"    value="-200"/>
-   <define name="HOVER_KD"    value="-100"/>
-   <define name="HOVER_KI"    value="-50"/>
+   <define name="HOVER_KP"    value="-300"/>
+   <define name="HOVER_KD"    value="-200"/>
+   <define name="HOVER_KI"    value="-75"/>
   </section>
 
 
diff --git a/conf/autopilot/booz2_autopilot.makefile b/conf/autopilot/booz2_autopilot.makefile
index ec3e061a83..21d169541b 100644
--- a/conf/autopilot/booz2_autopilot.makefile
+++ b/conf/autopilot/booz2_autopilot.makefile
@@ -114,13 +114,16 @@ ap.srcs += $(BOOZ_PRIV)/booz_trig_int.c
 ap.srcs += $(BOOZ_PRIV)/booz2_stabilization.c
 ap.srcs += $(BOOZ_PRIV)/booz2_stabilization_rate.c
 ap.srcs += $(BOOZ_PRIV)/booz2_stabilization_attitude.c
-#ap.srcs += $(BOOZ_PRIV)/booz_supervision_int.c
 
 ap.srcs += $(BOOZ_PRIV)/booz2_guidance_h.c
 ap.srcs += $(BOOZ_PRIV)/booz2_guidance_v.c
 ap.srcs += $(BOOZ_PRIV)/booz2_ins.c
+# vertical filter dummy complementary
+#ap.CFLAGS += -DUSE_VFD
+#  vertical filter float version
+ap.srcs += $(BOOZ_PRIV)/booz2_vf_float.c
+ap.CFLAGS += -DUSE_VFF -DDT_VFILTER="(1./512.)" -DFLOAT_T=float
 
-#ap.CFLAGS += -DNAV_ENAC
 ap.srcs += $(BOOZ_PRIV)/booz2_navigation.c
 
 
diff --git a/conf/messages.xml b/conf/messages.xml
index 394614b427..ca7bf5f83c 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -783,11 +783,10 @@
   </message>
 
   <message name="BOOZ2_VERT_LOOP" id="145">
-    <field name="z_sp"    type="int32"/>
-    <field name="z_meas"  type="int32"/>
-    <field name="zd_est"  type="int32"/>
-    <field name="sum_err" type="int32"/>
-    <field name="g_est"   type="int32"/>
+    <field name="z_sp"    type="int32" alt_unit="m"    alt_unit_coef="0.0039063"/>
+    <field name="z_meas"  type="int32" alt_unit="m"    alt_unit_coef="0.0039063"/>
+    <field name="zd_est"  type="int32" alt_unit="m/s"  alt_unit_coef="0.0000019"/>
+    <field name="sum_err" type="int32" />
     <field name="delta_t" type="int32"/>
   </message>
 
@@ -846,9 +845,15 @@
     <field name="ua_psi"      type="int32"/>
   </message>
 
+  <message name="BOOZ2_INS" id="151">
+    <field name="baro_alt"  type="int32" alt_unit="m"    alt_unit_coef="0.0039063"/>
+    <field name="ins_z"     type="int32" alt_unit="m"    alt_unit_coef="0.0039063"/>
+    <field name="ins_zd"    type="int32" alt_unit="m/s"  alt_unit_coef="0.0000019"/>
+    <field name="ins_zdd"   type="int32" alt_unit="m/s2" alt_unit_coef="0.0009766"/>
+  </message>
 
 
-  <message name="WT" id="151">
+  <message name="WT" id="152">
     <field name="rpm"      type="float"/>
   </message>
 
diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml
index 3da2e05ea7..632075d2fd 100644
--- a/conf/telemetry/telemetry_booz2.xml
+++ b/conf/telemetry/telemetry_booz2.xml
@@ -65,7 +65,8 @@
       <message name="DL_VALUE"                period="0.5"/>
       <message name="ALIVE"                   period="0.9"/>
       <message name="BOOZ2_VERT_LOOP"         period=".05"/>
-      <message name="BOOZ2_CMD"               period=".05"/>
+<!--      <message name="BOOZ2_CMD"               period=".05"/> -->
+      <message name="BOOZ2_INS"               period=".05"/>
     </mode>
 
     <mode name="h_loop">