diff --git a/sw/airborne/agl_vfilter.c b/sw/airborne/agl_vfilter.c
new file mode 100644
index 0000000000..25d9efd77c
--- /dev/null
+++ b/sw/airborne/agl_vfilter.c
@@ -0,0 +1,141 @@
+#include "agl_vfilter.h"
+
+#include CONFIG
+#include "airframe.h"
+#include "std.h"
+
+/*
+
+X = [ z zdot bias ]
+
+temps : 
+  predict 86us
+  update  46us      
+
+*/
+/* initial covariance diagonal */
+#define INIT_PXX 10.
+/* process noise */
+#define Qzz       0.001
+#define Qzdotzdot 0.001
+#define Qbiasbias 0.00001
+#define R 2.
+
+float agl_vf_z;
+float agl_vf_zdot;
+float agl_vf_bias;
+
+struct adc_buf agl_adc_buf;
+
+float agl_vf_P[STATE_SIZE][STATE_SIZE];
+
+float agl_vf_z_meas;
+
+void agl_vf_init(float init_z, float init_zdot, float init_bias) {
+  agl_vf_z = init_z;
+  agl_vf_zdot = init_zdot;
+  agl_vf_bias = init_bias;
+  int i, j;
+  for (i=0; i<STATE_SIZE; i++) {
+    for (j=0; j<STATE_SIZE; j++) 
+      agl_vf_P[i][j] = 0.;
+    agl_vf_P[i][i] = INIT_PXX;
+  }
+  adc_buf_channel(ADC_CHANNEL_TELEMETER, &agl_adc_buf, DEFAULT_AV_NB_SAMPLE);
+
+}
+
+
+/*
+
+ F = [ 1 dt -dt^2/2
+       0  1 -dt
+       0  0   1     ];
+   
+ B = [ dt^2/2 dt 0]';
+ 
+ Q = [ 0.01  0     0
+       0     0.01  0
+       0     0     0.001 ];
+ 
+ Xk1 = F * Xk0 + B * accel;
+ 
+ Pk1 = F * Pk0 * F' + Q;
+
+*/
+void agl_vf_predict(float accel) {
+  /* update state */
+  float u = accel + 9.81;
+  agl_vf_z = agl_vf_z + DT_VFILTER * agl_vf_zdot;
+  agl_vf_zdot = agl_vf_zdot + DT_VFILTER * ( u - agl_vf_bias); 
+  /* update covariance */
+  const float FPF00 = agl_vf_P[0][0] + DT_VFILTER * ( agl_vf_P[1][0] + agl_vf_P[0][1] + DT_VFILTER * agl_vf_P[1][1] );  
+  const float FPF01 = agl_vf_P[0][1] + DT_VFILTER * ( agl_vf_P[1][1] - agl_vf_P[0][2] - DT_VFILTER * agl_vf_P[1][2] );
+  const float FPF02 = agl_vf_P[0][2] + DT_VFILTER * ( agl_vf_P[1][2] );
+  const float FPF10 = agl_vf_P[1][0] + DT_VFILTER * (-agl_vf_P[2][0] + agl_vf_P[1][1] - DT_VFILTER * agl_vf_P[2][1] );  
+  const float FPF11 = agl_vf_P[1][1] + DT_VFILTER * (-agl_vf_P[2][1] - agl_vf_P[1][2] + DT_VFILTER * agl_vf_P[2][2] ); 
+  const float FPF12 = agl_vf_P[1][2] + DT_VFILTER * (-agl_vf_P[2][2] );
+  const float FPF20 = agl_vf_P[2][0] + DT_VFILTER * ( agl_vf_P[2][1] );
+  const float FPF21 = agl_vf_P[2][1] + DT_VFILTER * (-agl_vf_P[2][2] );
+  const float FPF22 = agl_vf_P[2][2];
+ 
+  agl_vf_P[0][0] = FPF00 + Qzz;
+  agl_vf_P[0][1] = FPF01;
+  agl_vf_P[0][2] = FPF02;
+  agl_vf_P[1][0] = FPF10;
+  agl_vf_P[1][1] = FPF11 + Qzdotzdot;
+  agl_vf_P[1][2] = FPF12;
+  agl_vf_P[2][0] = FPF20;
+  agl_vf_P[2][1] = FPF21;
+  agl_vf_P[2][2] = FPF22 + Qbiasbias;
+
+}
+/*
+  H = [1 0 0];
+  R = 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;
+*/
+void agl_vf_update(float z_meas) {
+  agl_vf_z_meas = z_meas;
+
+  const float y = z_meas - agl_vf_z;
+  const float S = agl_vf_P[0][0] + R;
+  const float K1 = agl_vf_P[0][0] * 1/S; 
+  const float K2 = agl_vf_P[1][0] * 1/S; 
+  const float K3 = agl_vf_P[2][0] * 1/S; 
+  
+  agl_vf_z    = agl_vf_z    + K1 * y; 
+  agl_vf_zdot = agl_vf_zdot + K2 * y; 
+  agl_vf_bias = agl_vf_bias + K3 * y; 
+
+  const float P11 = (1. - K1) * agl_vf_P[0][0];
+  const float P12 = (1. - K1) * agl_vf_P[0][1];
+  const float P13 = (1. - K1) * agl_vf_P[0][2];
+  const float P21 = -K2 * agl_vf_P[0][0] + agl_vf_P[1][0];
+  const float P22 = -K2 * agl_vf_P[0][1] + agl_vf_P[1][1];
+  const float P23 = -K2 * agl_vf_P[0][2] + agl_vf_P[1][2];
+  const float P31 = -K3 * agl_vf_P[0][0] + agl_vf_P[2][0];
+  const float P32 = -K3 * agl_vf_P[0][1] + agl_vf_P[2][1];
+  const float P33 = -K3 * agl_vf_P[0][2] + agl_vf_P[2][2];
+
+  agl_vf_P[0][0] = P11;
+  agl_vf_P[0][1] = P12;
+  agl_vf_P[0][2] = P13;
+  agl_vf_P[1][0] = P21;
+  agl_vf_P[1][1] = P22;
+  agl_vf_P[1][2] = P23;
+  agl_vf_P[2][0] = P31;
+  agl_vf_P[2][1] = P32;
+  agl_vf_P[2][2] = P33;
+
+}
+
diff --git a/sw/airborne/agl_vfilter.h b/sw/airborne/agl_vfilter.h
new file mode 100644
index 0000000000..91ce89532f
--- /dev/null
+++ b/sw/airborne/agl_vfilter.h
@@ -0,0 +1,21 @@
+#ifndef AGL_VFILTER_H
+#define AGL_VFILTER_H
+
+#include "adc.h"
+
+#define STATE_SIZE 3
+
+extern float agl_vf_z;
+extern float agl_vf_zdot;
+extern float agl_vf_bias;
+extern float agl_vf_P[STATE_SIZE][STATE_SIZE];
+
+extern struct adc_buf agl_adc_buf;
+
+extern float agl_vf_z_meas;
+
+extern void agl_vf_init(float z, float zdot, float bias);
+extern void agl_vf_predict(float accel);
+extern void agl_vf_update(float z_meas);
+
+#endif /* AGL_VFILTER_H */
diff --git a/sw/airborne/alt_vfilter.c b/sw/airborne/alt_vfilter.c
new file mode 100644
index 0000000000..f75507202d
--- /dev/null
+++ b/sw/airborne/alt_vfilter.c
@@ -0,0 +1,183 @@
+#include "alt_vfilter.h"
+
+/*
+
+X = [ z zdot bias ]
+
+temps : 
+  predict 86us
+  update  46us      
+
+*/
+/* initial covariance diagonal */
+#define INIT_PXX 10.
+/* process noise */
+#define Qzz       0.001
+#define Qzdotzdot 0.001
+#define Qbiasbias 0.00001
+#define R 2.
+
+float alt_vf_z;
+float alt_vf_zdot;
+float alt_vf_bias;
+
+float alt_vf_P[STATE_SIZE][STATE_SIZE];
+
+float alt_vf_z_meas;
+
+void alt_vf_init(float init_z, float init_zdot, float init_bias) {
+  alt_vf_z = init_z;
+  alt_vf_zdot = init_zdot;
+  alt_vf_bias = init_bias;
+  int i, j;
+  for (i=0; i<STATE_SIZE; i++) {
+    for (j=0; j<STATE_SIZE; j++) 
+      alt_vf_P[i][j] = 0.;
+    alt_vf_P[i][i] = INIT_PXX;
+  }
+
+}
+
+
+/*
+
+ F = [ 1 dt -dt^2/2
+       0  1 -dt
+       0  0   1     ];
+   
+ B = [ dt^2/2 dt 0]';
+ 
+ Q = [ 0.01  0     0
+       0     0.01  0
+       0     0     0.001 ];
+ 
+ Xk1 = F * Xk0 + B * accel;
+ 
+ Pk1 = F * Pk0 * F' + Q;
+
+*/
+void alt_vf_predict(float accel) {
+  /* update state */
+  float u = accel + 9.81;
+  alt_vf_z = alt_vf_z + DT_VFILTER * alt_vf_zdot;
+  alt_vf_zdot = alt_vf_zdot + DT_VFILTER * ( u - alt_vf_bias); 
+  /* update covariance */
+  const float FPF00 = alt_vf_P[0][0] + DT_VFILTER * ( alt_vf_P[1][0] + alt_vf_P[0][1] + DT_VFILTER * alt_vf_P[1][1] );  
+  const float FPF01 = alt_vf_P[0][1] + DT_VFILTER * ( alt_vf_P[1][1] - alt_vf_P[0][2] - DT_VFILTER * alt_vf_P[1][2] );
+  const float FPF02 = alt_vf_P[0][2] + DT_VFILTER * ( alt_vf_P[1][2] );
+  const float FPF10 = alt_vf_P[1][0] + DT_VFILTER * (-alt_vf_P[2][0] + alt_vf_P[1][1] - DT_VFILTER * alt_vf_P[2][1] );  
+  const float FPF11 = alt_vf_P[1][1] + DT_VFILTER * (-alt_vf_P[2][1] - alt_vf_P[1][2] + DT_VFILTER * alt_vf_P[2][2] ); 
+  const float FPF12 = alt_vf_P[1][2] + DT_VFILTER * (-alt_vf_P[2][2] );
+  const float FPF20 = alt_vf_P[2][0] + DT_VFILTER * ( alt_vf_P[2][1] );
+  const float FPF21 = alt_vf_P[2][1] + DT_VFILTER * (-alt_vf_P[2][2] );
+  const float FPF22 = alt_vf_P[2][2];
+ 
+  alt_vf_P[0][0] = FPF00 + Qzz;
+  alt_vf_P[0][1] = FPF01;
+  alt_vf_P[0][2] = FPF02;
+  alt_vf_P[1][0] = FPF10;
+  alt_vf_P[1][1] = FPF11 + Qzdotzdot;
+  alt_vf_P[1][2] = FPF12;
+  alt_vf_P[2][0] = FPF20;
+  alt_vf_P[2][1] = FPF21;
+  alt_vf_P[2][2] = FPF22 + Qbiasbias;
+
+}
+
+/*
+  H = [1 0 0];
+  R = 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;
+*/
+void alt_vf_update_z(float z_meas) {
+  alt_vf_z_meas = z_meas;
+
+  const float y = z_meas - alt_vf_z;
+  const float S = alt_vf_P[0][0] + R;
+  const float K1 = alt_vf_P[0][0] * 1/S; 
+  const float K2 = alt_vf_P[1][0] * 1/S; 
+  const float K3 = alt_vf_P[2][0] * 1/S; 
+  
+  alt_vf_z    = alt_vf_z    + K1 * y; 
+  alt_vf_zdot = alt_vf_zdot + K2 * y; 
+  alt_vf_bias = alt_vf_bias + K3 * y; 
+
+  const float P11 = (1. - K1) * alt_vf_P[0][0];
+  const float P12 = (1. - K1) * alt_vf_P[0][1];
+  const float P13 = (1. - K1) * alt_vf_P[0][2];
+  const float P21 = -K2 * alt_vf_P[0][0] + alt_vf_P[1][0];
+  const float P22 = -K2 * alt_vf_P[0][1] + alt_vf_P[1][1];
+  const float P23 = -K2 * alt_vf_P[0][2] + alt_vf_P[1][2];
+  const float P31 = -K3 * alt_vf_P[0][0] + alt_vf_P[2][0];
+  const float P32 = -K3 * alt_vf_P[0][1] + alt_vf_P[2][1];
+  const float P33 = -K3 * alt_vf_P[0][2] + alt_vf_P[2][2];
+
+  alt_vf_P[0][0] = P11;
+  alt_vf_P[0][1] = P12;
+  alt_vf_P[0][2] = P13;
+  alt_vf_P[1][0] = P21;
+  alt_vf_P[1][1] = P22;
+  alt_vf_P[1][2] = P23;
+  alt_vf_P[2][0] = P31;
+  alt_vf_P[2][1] = P32;
+  alt_vf_P[2][2] = P33;
+
+}
+
+/*
+  H = [0 1 0];
+  R = 0.1;
+  // state residual
+  yd = vz - H * Xm;
+  // covariance residual
+  S = H*Pm*H' + R;
+  // kalman gain
+  K = Pm*H'*inv(S);
+  // update state
+  Xp = Xm + K*yd;
+  // update covariance
+  Pp = Pm - K*H*Pm;
+*/
+void alt_vf_update_vz(float vz) {
+  const float yd = vz - alt_vf_zdot;
+  const float S = alt_vf_P[1][1] + R;
+  const float K1 = alt_vf_P[0][1] * 1/S; 
+  const float K2 = alt_vf_P[1][1] * 1/S; 
+  const float K3 = alt_vf_P[2][1] * 1/S; 
+  
+  alt_vf_z    = alt_vf_z    + K1 * yd; 
+  alt_vf_zdot = alt_vf_zdot + K2 * yd; 
+  alt_vf_bias = alt_vf_bias + K3 * yd; 
+
+  const float P11 = -K1 * alt_vf_P[1][0] + alt_vf_P[0][0];
+  const float P12 = -K1 * alt_vf_P[1][1] + alt_vf_P[0][1];
+  const float P13 = -K1 * alt_vf_P[1][2] + alt_vf_P[0][2];
+  const float P21 = (1. - K2) * alt_vf_P[1][0];
+  const float P22 = (1. - K2) * alt_vf_P[1][1];
+  const float P23 = (1. - K2) * alt_vf_P[1][2];
+  const float P31 = -K3 * alt_vf_P[1][0] + alt_vf_P[2][0];
+  const float P32 = -K3 * alt_vf_P[1][1] + alt_vf_P[2][1];
+  const float P33 = -K3 * alt_vf_P[1][2] + alt_vf_P[2][2];
+
+  alt_vf_P[0][0] = P11;
+  alt_vf_P[0][1] = P12;
+  alt_vf_P[0][2] = P13;
+  alt_vf_P[1][0] = P21;
+  alt_vf_P[1][1] = P22;
+  alt_vf_P[1][2] = P23;
+  alt_vf_P[2][0] = P31;
+  alt_vf_P[2][1] = P32;
+  alt_vf_P[2][2] = P33;
+
+}
+
+
diff --git a/sw/airborne/alt_vfilter.h b/sw/airborne/alt_vfilter.h
new file mode 100644
index 0000000000..1e779eb602
--- /dev/null
+++ b/sw/airborne/alt_vfilter.h
@@ -0,0 +1,45 @@
+#ifndef ALT_VFILTER_H
+#define ALT_VFILTER_H
+
+#define STATE_SIZE 3
+
+extern float alt_vf_z;
+extern float alt_vf_zdot;
+extern float alt_vf_bias;
+extern float alt_vf_P[STATE_SIZE][STATE_SIZE];
+
+extern float alt_vf_z_meas;
+
+extern void alt_vf_init(float z, float zdot, float bias);
+extern void alt_vf_predict(float accel);
+extern void alt_vf_update_z(float z_meas);
+extern void alt_vf_update_vz(float vz);
+
+extern void alt_vf_periodic_task(void);
+
+
+#ifdef USE_BARO_MS5534A
+
+#include "baro_MS5534A.h"
+
+#define AltVFliterEvent() { \
+  if (spi_message_received) { \
+    /* Got a message on SPI. */ \
+    spi_message_received = FALSE; \
+    baro_MS5534A_event_task(); \
+    if (baro_MS5534A_available) { \
+      baro_MS5534A_available = FALSE; \
+      baro_MS5534A_z = ground_alt +((float)baro_MS5534A_ground_pressure - baro_MS5534A_pressure)*0.084; \
+      if (alt_baro_enabled) { \
+        alt_vf_update_z(-baro_MS5534A_z); \
+      } \
+    } \
+  } \
+}
+
+#else
+#define AltVFliterEvent() {}
+#endif
+
+
+#endif /* ALT_VFILTER_H */