Lead Filter: simplified calculation

libraries/AP_LeadFilter/AP_LeadFilter.cpp

@@ -17,7 +17,7 @@
 
 #include "AP_LeadFilter.h"
 
-// setup the control preferences
+/*
 int32_t
 AP_LeadFilter::get_position(int32_t pos, int16_t vel)
 {
@@ -27,3 +27,14 @@ AP_LeadFilter::get_position(int32_t pos, int16_t vel)
 	_last_velocity = vel;
 	return pos;
 }
+*/
+
+// setup the control preferences
+int32_t
+AP_LeadFilter::get_position(int32_t pos, int16_t vel)
+{
+	// assumes a 1 second delay in the GPS
+	int16_t acc = vel - _last_velocity;
+	_last_velocity = vel;
+	return pos + vel + acc;
+}