diff --git a/libraries/AP_LeadFilter/AP_LeadFilter.cpp b/libraries/AP_LeadFilter/AP_LeadFilter.cpp
index 19197c00c1..d4d296822f 100644
--- a/libraries/AP_LeadFilter/AP_LeadFilter.cpp
+++ b/libraries/AP_LeadFilter/AP_LeadFilter.cpp
@@ -40,5 +40,5 @@ AP_LeadFilter::get_position(int32_t pos, int16_t vel)
     // store velocity for next iteration
     _last_velocity = vel;
 
-    return pos + vel + accel_contribution;
+    return pos + vel_contribution + accel_contribution;
 }