Added a crude Lead Filter. Needs some fine tuning, lag specification, scaling. Works great on 4hz GPS such as Mtek.

libraries/AP_LeadFilter/AP_LeadFilter.cpp

@@ -0,0 +1,29 @@
+/*
+	AP_LeadFilter.cpp - GPS lag remover library for Arduino
+	Code by Jason Short. DIYDrones.com
+
+	This library is free software; you can redistribute it and / or
+		modify it under the terms of the GNU Lesser General Public
+		License as published by the Free Software Foundation; either
+		version 2.1 of the License, or (at your option) any later version.
+
+*/
+
+#if defined(ARDUINO) && ARDUINO >= 100
+	#include "Arduino.h"
+#else
+	#include "WProgram.h"
+#endif
+
+#include "AP_LeadFilter.h"
+
+// setup the control preferences
+int32_t
+AP_LeadFilter::get_position(int32_t pos, int16_t vel)
+{
+	vel = (_last_velocity + vel) / 2;
+	pos += vel;
+	pos += (vel - _last_velocity);
+	_last_velocity = vel;
+	return pos;
+}

libraries/AP_LeadFilter/AP_LeadFilter.h

@@ -0,0 +1,29 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file	RC_Channel.h
+/// @brief	RC_Channel manager, with EEPROM-backed storage of constants.
+
+#ifndef AP_LeadFilter_h
+#define AP_LeadFilter_h
+
+#include <AP_LeadFilter.h>
+
+/// @class	AP_LeadFilter
+/// @brief	Object managing GPS lag
+class AP_LeadFilter{
+  public:
+	/// Constructor
+	///
+	///
+       AP_LeadFilter() :
+		_last_velocity(0) {}
+
+	// setup min and max radio values in CLI
+	int32_t 	get_position(int32_t pos, int16_t vel);
+
+  private:
+	int16_t		_last_velocity;
+
+};
+
+#endif

libraries/AP_LeadFilter/examples/AP_LeadFilter/AP_LeadFilter.pde

@@ -0,0 +1,63 @@
+/*
+	Example of AP Lead_Filter library.
+	Code by Jason Short. 2010
+	DIYDrones.com
+
+*/
+
+#include <FastSerial.h>
+#include <AP_Common.h>		// ArduPilot Mega Common Library
+#include <Arduino_Mega_ISR_Registry.h>
+#include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+#include <AP_LeadFilter.h>	// GPS Lead filter
+
+// define APM1 or APM2
+#define APM_HARDWARE_APM1	1
+#define APM_HARDWARE_APM2	2
+
+// set your hardware type here
+#define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
+
+Arduino_Mega_ISR_Registry isr_registry;
+
+////////////////////////////////////////////////////////////////////////////////
+// Serial ports
+////////////////////////////////////////////////////////////////////////////////
+//
+// Note that FastSerial port buffers are allocated at ::begin time,
+// so there is not much of a penalty to defining ports that we don't
+// use.
+//
+FastSerialPort0(Serial);        // FTDI/console
+FastSerialPort1(Serial1);       // GPS port
+FastSerialPort3(Serial3);       // Telemetry port
+
+////////////////////////////////////////////
+// RC Hardware
+////////////////////////////////////////////
+#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
+    APM_RC_APM2 APM_RC;
+#else
+    APM_RC_APM1 APM_RC;
+#endif
+
+AP_LeadFilter xLeadFilter;	// GPS lag filter
+
+void setup()
+{
+	Serial.begin(115200);
+	Serial.println("ArduPilot RC Channel test");
+
+	delay(500);
+
+	int32_t temp = xLeadFilter.get_position(0, 100);
+	Serial.printf("temp %ld \n", temp);
+}
+
+void loop()
+{
+}
+
+
+
+

libraries/AP_LeadFilter/examples/AP_LeadFilter/Makefile

@@ -0,0 +1,2 @@
+BOARD	=	mega
+include ../../../AP_Common/Arduino.mk