diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
index 3444ef2c89..ee0958e74e 100644
--- a/sw/airborne/subsystems/ins/ins_int.c
+++ b/sw/airborne/subsystems/ins/ins_int.c
@@ -518,12 +518,13 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
 }
 
 static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
-                       uint32_t stamp __attribute__((unused)),
+                       uint32_t stamp,
                        float x, float y, float z,
                        float noise __attribute__((unused)))
 {
 
   struct FloatVect3 vel_body = {x, y, z};
+  static uint32_t last_stamp = 0;
 
   /* rotate velocity estimate to nav/ltp frame */
   struct FloatQuat q_b2n = *stateGetNedToBodyQuat_f();
@@ -540,8 +541,12 @@ static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
 #else
   ins_int.ltp_speed.x = SPEED_BFP_OF_REAL(vel_ned.x);
   ins_int.ltp_speed.y = SPEED_BFP_OF_REAL(vel_ned.y);
-  ins_int.ltp_pos.x = ins_int.ltp_pos.x + POS_BFP_OF_REAL(vel_ned.x);
-  ins_int.ltp_pos.y = ins_int.ltp_pos.y + POS_BFP_OF_REAL(vel_ned.y);
+  if (last_stamp > 0) {
+    float dt = (float)(stamp - last_stamp) * 1e-6;
+    ins_int.ltp_pos.x = ins_int.ltp_pos.x + POS_BFP_OF_REAL(dt * vel_ned.x);
+    ins_int.ltp_pos.y = ins_int.ltp_pos.y + POS_BFP_OF_REAL(dt * vel_ned.y);
+  }
+  last_stamp = stamp;
 #endif
 
   ins_ned_to_state();